1143 lines
43 KiB
C++
1143 lines
43 KiB
C++
#include <algorithm>
|
|
#include <cmath>
|
|
#include <stdexcept>
|
|
#include <vector>
|
|
|
|
#include <emscripten/bind.h>
|
|
|
|
#include "GCS.h"
|
|
|
|
namespace
|
|
{
|
|
std::vector<double> solveLineDistance(
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
double targetLength,
|
|
bool vertical
|
|
)
|
|
{
|
|
GCS::Line line;
|
|
line.p1 = GCS::Point(&startX, &startY);
|
|
line.p2 = GCS::Point(&endX, &endY);
|
|
GCS::System system;
|
|
double fixedStartX = startX;
|
|
double fixedStartY = startY;
|
|
system.addConstraintCoordinateX(line.p1, &fixedStartX, 1, true);
|
|
system.addConstraintCoordinateY(line.p1, &fixedStartY, 2, true);
|
|
if (vertical) {
|
|
system.addConstraintVertical(line, 3, true);
|
|
} else {
|
|
system.addConstraintHorizontal(line, 3, true);
|
|
}
|
|
system.addConstraintP2PDistance(line.p1, line.p2, &targetLength, 4, true);
|
|
GCS::VEC_pD parameters {&startX, &startY, &endX, &endY};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = std::hypot(endX - startX, endY - startY) - targetLength;
|
|
return {static_cast<double>(status), startX, startY, endX, endY, residual};
|
|
}
|
|
|
|
std::vector<double> solveHorizontalDistance(
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
double targetLength
|
|
)
|
|
{
|
|
return solveLineDistance(startX, startY, endX, endY, targetLength, false);
|
|
}
|
|
|
|
std::vector<double> solveVerticalDistance(
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
double targetLength
|
|
)
|
|
{
|
|
return solveLineDistance(startX, startY, endX, endY, targetLength, true);
|
|
}
|
|
|
|
std::vector<double> solveDistanceX(
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
double targetDistance
|
|
)
|
|
{
|
|
GCS::Line line;
|
|
line.p1 = GCS::Point(&startX, &startY);
|
|
line.p2 = GCS::Point(&endX, &endY);
|
|
GCS::System system;
|
|
double fixedStartX = startX;
|
|
double fixedStartY = startY;
|
|
system.addConstraintCoordinateX(line.p1, &fixedStartX, 1, true);
|
|
system.addConstraintCoordinateY(line.p1, &fixedStartY, 2, true);
|
|
system.addConstraintHorizontal(line, 3, true);
|
|
system.addConstraintDifference(&startX, &endX, &targetDistance, 4, true);
|
|
GCS::VEC_pD parameters {&startX, &startY, &endX, &endY};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = (endX - startX) - targetDistance;
|
|
return {static_cast<double>(status), startX, startY, endX, endY, residual};
|
|
}
|
|
|
|
std::vector<double> solveDistanceY(
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
double targetDistance
|
|
)
|
|
{
|
|
GCS::Line line;
|
|
line.p1 = GCS::Point(&startX, &startY);
|
|
line.p2 = GCS::Point(&endX, &endY);
|
|
GCS::System system;
|
|
double fixedStartX = startX;
|
|
double fixedStartY = startY;
|
|
system.addConstraintCoordinateX(line.p1, &fixedStartX, 1, true);
|
|
system.addConstraintCoordinateY(line.p1, &fixedStartY, 2, true);
|
|
system.addConstraintVertical(line, 3, true);
|
|
system.addConstraintDifference(&startY, &endY, &targetDistance, 4, true);
|
|
GCS::VEC_pD parameters {&startX, &startY, &endX, &endY};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = (endY - startY) - targetDistance;
|
|
return {static_cast<double>(status), startX, startY, endX, endY, residual};
|
|
}
|
|
|
|
std::vector<double> solveAngle(
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
double targetLength,
|
|
double targetAngle
|
|
)
|
|
{
|
|
GCS::Line line;
|
|
line.p1 = GCS::Point(&startX, &startY);
|
|
line.p2 = GCS::Point(&endX, &endY);
|
|
GCS::System system;
|
|
double fixedStartX = startX;
|
|
double fixedStartY = startY;
|
|
system.addConstraintCoordinateX(line.p1, &fixedStartX, 1, true);
|
|
system.addConstraintCoordinateY(line.p1, &fixedStartY, 2, true);
|
|
system.addConstraintP2PDistance(line.p1, line.p2, &targetLength, 3, true);
|
|
system.addConstraintP2PAngle(line.p1, line.p2, &targetAngle, 4, true);
|
|
GCS::VEC_pD parameters {&startX, &startY, &endX, &endY};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double lengthResidual = std::hypot(endX - startX, endY - startY) - targetLength;
|
|
const double angleResidual = std::atan2(endY - startY, endX - startX) - targetAngle;
|
|
const double normalizedAngleResidual = std::atan2(std::sin(angleResidual), std::cos(angleResidual));
|
|
return {static_cast<double>(status), startX, startY, endX, endY, lengthResidual, normalizedAngleResidual};
|
|
}
|
|
|
|
std::vector<double> solveCircleRadius(
|
|
double centerX,
|
|
double centerY,
|
|
double radius,
|
|
double targetRadius
|
|
)
|
|
{
|
|
GCS::Circle circle;
|
|
circle.center = GCS::Point(¢erX, ¢erY);
|
|
circle.rad = &radius;
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
system.addConstraintCoordinateX(circle.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(circle.center, &fixedCenterY, 2, true);
|
|
system.addConstraintCircleRadius(circle, &targetRadius, 3, true);
|
|
GCS::VEC_pD parameters {¢erX, ¢erY, &radius};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = radius - targetRadius;
|
|
return {static_cast<double>(status), centerX, centerY, radius, residual};
|
|
}
|
|
|
|
std::vector<double> solveCircleDiameter(
|
|
double centerX,
|
|
double centerY,
|
|
double radius,
|
|
double targetDiameter
|
|
)
|
|
{
|
|
GCS::Circle circle;
|
|
circle.center = GCS::Point(¢erX, ¢erY);
|
|
circle.rad = &radius;
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
system.addConstraintCoordinateX(circle.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(circle.center, &fixedCenterY, 2, true);
|
|
system.addConstraintCircleDiameter(circle, &targetDiameter, 3, true);
|
|
GCS::VEC_pD parameters {¢erX, ¢erY, &radius};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = radius * 2.0 - targetDiameter;
|
|
return {static_cast<double>(status), centerX, centerY, radius, residual};
|
|
}
|
|
|
|
std::vector<double> solveEqualLines(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY
|
|
)
|
|
{
|
|
GCS::Line first;
|
|
first.p1 = GCS::Point(&firstStartX, &firstStartY);
|
|
first.p2 = GCS::Point(&firstEndX, &firstEndY);
|
|
GCS::Line second;
|
|
second.p1 = GCS::Point(&secondStartX, &secondStartY);
|
|
second.p2 = GCS::Point(&secondEndX, &secondEndY);
|
|
GCS::System system;
|
|
double fixedFirstStartX = firstStartX;
|
|
double fixedFirstStartY = firstStartY;
|
|
double fixedFirstEndX = firstEndX;
|
|
double fixedFirstEndY = firstEndY;
|
|
double fixedSecondStartX = secondStartX;
|
|
double fixedSecondStartY = secondStartY;
|
|
system.addConstraintCoordinateX(first.p1, &fixedFirstStartX, 1, true);
|
|
system.addConstraintCoordinateY(first.p1, &fixedFirstStartY, 2, true);
|
|
system.addConstraintCoordinateX(first.p2, &fixedFirstEndX, 3, true);
|
|
system.addConstraintCoordinateY(first.p2, &fixedFirstEndY, 4, true);
|
|
system.addConstraintCoordinateX(second.p1, &fixedSecondStartX, 5, true);
|
|
system.addConstraintCoordinateY(second.p1, &fixedSecondStartY, 6, true);
|
|
system.addConstraintEqualLength(first, second, 7, true);
|
|
GCS::VEC_pD parameters {
|
|
&firstStartX, &firstStartY, &firstEndX, &firstEndY,
|
|
&secondStartX, &secondStartY, &secondEndX, &secondEndY
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double firstLength = std::hypot(firstEndX - firstStartX, firstEndY - firstStartY);
|
|
const double secondLength = std::hypot(secondEndX - secondStartX, secondEndY - secondStartY);
|
|
return {
|
|
static_cast<double>(status),
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
secondLength - firstLength
|
|
};
|
|
}
|
|
|
|
std::vector<double> solveEqualCircles(
|
|
double firstCenterX,
|
|
double firstCenterY,
|
|
double firstRadius,
|
|
double secondCenterX,
|
|
double secondCenterY,
|
|
double secondRadius
|
|
)
|
|
{
|
|
GCS::Circle first;
|
|
first.center = GCS::Point(&firstCenterX, &firstCenterY);
|
|
first.rad = &firstRadius;
|
|
GCS::Circle second;
|
|
second.center = GCS::Point(&secondCenterX, &secondCenterY);
|
|
second.rad = &secondRadius;
|
|
GCS::System system;
|
|
double fixedFirstCenterX = firstCenterX;
|
|
double fixedFirstCenterY = firstCenterY;
|
|
double fixedSecondCenterX = secondCenterX;
|
|
double fixedSecondCenterY = secondCenterY;
|
|
double fixedFirstRadius = firstRadius;
|
|
system.addConstraintCoordinateX(first.center, &fixedFirstCenterX, 1, true);
|
|
system.addConstraintCoordinateY(first.center, &fixedFirstCenterY, 2, true);
|
|
system.addConstraintCoordinateX(second.center, &fixedSecondCenterX, 3, true);
|
|
system.addConstraintCoordinateY(second.center, &fixedSecondCenterY, 4, true);
|
|
system.addConstraintEqual(first.rad, &fixedFirstRadius, 5, true);
|
|
system.addConstraintEqualRadius(first, second, 6, true);
|
|
GCS::VEC_pD parameters {
|
|
&firstCenterX, &firstCenterY, &firstRadius,
|
|
&secondCenterX, &secondCenterY, &secondRadius
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = secondRadius - firstRadius;
|
|
return {
|
|
static_cast<double>(status),
|
|
firstCenterX, firstCenterY, firstRadius,
|
|
secondCenterX, secondCenterY, secondRadius,
|
|
residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solveTangentCircles(
|
|
double firstCenterX,
|
|
double firstCenterY,
|
|
double firstRadius,
|
|
double secondCenterX,
|
|
double secondCenterY,
|
|
double secondRadius
|
|
)
|
|
{
|
|
GCS::Circle first;
|
|
first.center = GCS::Point(&firstCenterX, &firstCenterY);
|
|
first.rad = &firstRadius;
|
|
GCS::Circle second;
|
|
second.center = GCS::Point(&secondCenterX, &secondCenterY);
|
|
second.rad = &secondRadius;
|
|
GCS::System system;
|
|
double fixedFirstCenterX = firstCenterX;
|
|
double fixedFirstCenterY = firstCenterY;
|
|
double fixedSecondCenterX = secondCenterX;
|
|
double fixedSecondCenterY = secondCenterY;
|
|
double fixedFirstRadius = firstRadius;
|
|
system.addConstraintCoordinateX(first.center, &fixedFirstCenterX, 1, true);
|
|
system.addConstraintCoordinateY(first.center, &fixedFirstCenterY, 2, true);
|
|
system.addConstraintCoordinateX(second.center, &fixedSecondCenterX, 3, true);
|
|
system.addConstraintCoordinateY(second.center, &fixedSecondCenterY, 4, true);
|
|
system.addConstraintEqual(first.rad, &fixedFirstRadius, 5, true);
|
|
system.addConstraintTangent(first, second, 6, true);
|
|
GCS::VEC_pD parameters {
|
|
&firstCenterX, &firstCenterY, &firstRadius,
|
|
&secondCenterX, &secondCenterY, &secondRadius
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double centerDistance = std::hypot(secondCenterX - firstCenterX, secondCenterY - firstCenterY);
|
|
const double residual = centerDistance - firstRadius - secondRadius;
|
|
return {
|
|
static_cast<double>(status),
|
|
firstCenterX, firstCenterY, firstRadius,
|
|
secondCenterX, secondCenterY, secondRadius,
|
|
residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solvePointSymmetry(
|
|
double firstX,
|
|
double firstY,
|
|
double secondX,
|
|
double secondY,
|
|
double centerX,
|
|
double centerY
|
|
)
|
|
{
|
|
GCS::Point first(&firstX, &firstY);
|
|
GCS::Point second(&secondX, &secondY);
|
|
GCS::Point center(¢erX, ¢erY);
|
|
GCS::System system;
|
|
double fixedFirstX = firstX;
|
|
double fixedFirstY = firstY;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
system.addConstraintCoordinateX(first, &fixedFirstX, 1, true);
|
|
system.addConstraintCoordinateY(first, &fixedFirstY, 2, true);
|
|
system.addConstraintCoordinateX(center, &fixedCenterX, 3, true);
|
|
system.addConstraintCoordinateY(center, &fixedCenterY, 4, true);
|
|
system.addConstraintP2PSymmetric(first, second, center, 5, true);
|
|
GCS::VEC_pD parameters {&firstX, &firstY, &secondX, &secondY, ¢erX, ¢erY};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = std::hypot(
|
|
(firstX + secondX) * 0.5 - centerX,
|
|
(firstY + secondY) * 0.5 - centerY
|
|
);
|
|
return {
|
|
static_cast<double>(status),
|
|
firstX, firstY,
|
|
secondX, secondY,
|
|
centerX, centerY,
|
|
residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solvePointOnLine(
|
|
double pointX,
|
|
double pointY,
|
|
double startX,
|
|
double startY,
|
|
double endX,
|
|
double endY,
|
|
bool vertical
|
|
)
|
|
{
|
|
GCS::Point point(&pointX, &pointY);
|
|
GCS::Line line;
|
|
line.p1 = GCS::Point(&startX, &startY);
|
|
line.p2 = GCS::Point(&endX, &endY);
|
|
GCS::System system;
|
|
double fixedStartX = startX;
|
|
double fixedStartY = startY;
|
|
double fixedEndX = endX;
|
|
double fixedEndY = endY;
|
|
system.addConstraintCoordinateX(line.p1, &fixedStartX, 1, true);
|
|
system.addConstraintCoordinateY(line.p1, &fixedStartY, 2, true);
|
|
system.addConstraintCoordinateX(line.p2, &fixedEndX, 3, true);
|
|
system.addConstraintCoordinateY(line.p2, &fixedEndY, 4, true);
|
|
if (vertical) {
|
|
system.addConstraintVertical(line, 5, true);
|
|
system.addConstraintCoordinateY(point, &pointY, 6, true);
|
|
} else {
|
|
system.addConstraintHorizontal(line, 5, true);
|
|
system.addConstraintCoordinateX(point, &pointX, 6, true);
|
|
}
|
|
system.addConstraintPointOnLine(point, line, 7, true);
|
|
GCS::VEC_pD parameters {&pointX, &pointY, &startX, &startY, &endX, &endY};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double dx = endX - startX;
|
|
const double dy = endY - startY;
|
|
const double length = std::hypot(dx, dy);
|
|
const double residual = length == 0.0
|
|
? 1.0
|
|
: std::abs((pointX - startX) * dy - (pointY - startY) * dx) / length;
|
|
return {static_cast<double>(status), pointX, pointY, startX, startY, endX, endY, residual};
|
|
}
|
|
|
|
std::vector<double> solvePointOnCircle(
|
|
double pointX,
|
|
double pointY,
|
|
double centerX,
|
|
double centerY,
|
|
double radius
|
|
)
|
|
{
|
|
GCS::Point point(&pointX, &pointY);
|
|
GCS::Circle circle;
|
|
circle.center = GCS::Point(¢erX, ¢erY);
|
|
circle.rad = &radius;
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
double fixedRadius = radius;
|
|
system.addConstraintCoordinateX(circle.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(circle.center, &fixedCenterY, 2, true);
|
|
system.addConstraintEqual(circle.rad, &fixedRadius, 3, true);
|
|
double fixedPointX = pointX;
|
|
system.addConstraintCoordinateX(point, &fixedPointX, 4, true);
|
|
system.addConstraintPointOnCircle(point, circle, 5, true);
|
|
GCS::VEC_pD parameters {&pointX, &pointY, ¢erX, ¢erY, &radius};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = std::hypot(pointX - centerX, pointY - centerY) - radius;
|
|
return {static_cast<double>(status), pointX, pointY, centerX, centerY, radius, residual};
|
|
}
|
|
|
|
std::vector<double> solvePointOnArc(
|
|
double pointX,
|
|
double pointY,
|
|
double centerX,
|
|
double centerY,
|
|
double radius,
|
|
double startAngle,
|
|
double endAngle
|
|
)
|
|
{
|
|
if (!(radius > 0.0)) throw std::invalid_argument("Arc radius must be positive.");
|
|
GCS::Point point(&pointX, &pointY);
|
|
GCS::Arc arc;
|
|
arc.center = GCS::Point(¢erX, ¢erY);
|
|
arc.rad = &radius;
|
|
arc.startAngle = &startAngle;
|
|
arc.endAngle = &endAngle;
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
double fixedRadius = radius;
|
|
double fixedPointX = pointX;
|
|
system.addConstraintCoordinateX(arc.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(arc.center, &fixedCenterY, 2, true);
|
|
system.addConstraintEqual(arc.rad, &fixedRadius, 3, true);
|
|
system.addConstraintCoordinateX(point, &fixedPointX, 4, true);
|
|
system.addConstraintPointOnArc(point, arc, 5, true);
|
|
GCS::VEC_pD parameters {&pointX, &pointY, ¢erX, ¢erY, &radius};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = std::hypot(pointX - centerX, pointY - centerY) - radius;
|
|
return {static_cast<double>(status), pointX, pointY, centerX, centerY, radius, startAngle, endAngle, residual};
|
|
}
|
|
|
|
std::vector<double> solvePointOnEllipse(
|
|
double pointX,
|
|
double pointY,
|
|
double centerX,
|
|
double centerY,
|
|
double focusX,
|
|
double focusY,
|
|
double minorRadius
|
|
)
|
|
{
|
|
if (!(minorRadius > 0.0)) throw std::invalid_argument("Ellipse minor radius must be positive.");
|
|
const double focusDistance = std::hypot(focusX - centerX, focusY - centerY);
|
|
const double majorRadius = std::hypot(focusDistance, minorRadius);
|
|
if (!(majorRadius > 0.0) || focusDistance >= majorRadius) throw std::invalid_argument("Ellipse focus and radius are invalid.");
|
|
GCS::Point point(&pointX, &pointY);
|
|
GCS::Ellipse ellipse;
|
|
ellipse.center = GCS::Point(¢erX, ¢erY);
|
|
ellipse.focus1 = GCS::Point(&focusX, &focusY);
|
|
ellipse.radmin = &minorRadius;
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
double fixedFocusX = focusX;
|
|
double fixedFocusY = focusY;
|
|
double fixedMinorRadius = minorRadius;
|
|
double fixedPointX = pointX;
|
|
system.addConstraintCoordinateX(ellipse.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(ellipse.center, &fixedCenterY, 2, true);
|
|
system.addConstraintCoordinateX(ellipse.focus1, &fixedFocusX, 3, true);
|
|
system.addConstraintCoordinateY(ellipse.focus1, &fixedFocusY, 4, true);
|
|
system.addConstraintEqual(ellipse.radmin, &fixedMinorRadius, 5, true);
|
|
system.addConstraintCoordinateX(point, &fixedPointX, 6, true);
|
|
system.addConstraintPointOnEllipse(point, ellipse, 7, true);
|
|
GCS::VEC_pD parameters {&pointX, &pointY, ¢erX, ¢erY, &focusX, &focusY, &minorRadius};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double focus2X = centerX - (focusX - centerX);
|
|
const double focus2Y = centerY - (focusY - centerY);
|
|
const double residual = std::hypot(pointX - focusX, pointY - focusY) + std::hypot(pointX - focus2X, pointY - focus2Y) - 2.0 * majorRadius;
|
|
return {static_cast<double>(status), pointX, pointY, centerX, centerY, focusX, focusY, minorRadius, residual};
|
|
}
|
|
|
|
std::vector<double> solveEllipseInternalAlignment(
|
|
double centerX,
|
|
double centerY,
|
|
double majorRadius,
|
|
double minorRadius,
|
|
double rotation,
|
|
int alignmentType
|
|
)
|
|
{
|
|
if (!std::isfinite(centerX) || !std::isfinite(centerY) || !std::isfinite(rotation)
|
|
|| !(majorRadius > minorRadius) || !(minorRadius > 0.0)) {
|
|
throw std::invalid_argument("InternalAlignment requires a finite non-circular ellipse.");
|
|
}
|
|
if (alignmentType < 1 || alignmentType > 4) {
|
|
throw std::invalid_argument("Ellipse InternalAlignment type must be in the range 1..4.");
|
|
}
|
|
const double focalDistance = std::sqrt(majorRadius * majorRadius - minorRadius * minorRadius);
|
|
const double majorX = std::cos(rotation);
|
|
const double majorY = std::sin(rotation);
|
|
const double minorX = -majorY;
|
|
const double minorY = majorX;
|
|
double focusX = centerX + focalDistance * majorX;
|
|
double focusY = centerY + focalDistance * majorY;
|
|
double helperStartX;
|
|
double helperStartY;
|
|
double helperEndX;
|
|
double helperEndY;
|
|
if (alignmentType == 1) {
|
|
helperStartX = centerX + majorRadius * majorX;
|
|
helperStartY = centerY + majorRadius * majorY;
|
|
helperEndX = centerX - majorRadius * majorX;
|
|
helperEndY = centerY - majorRadius * majorY;
|
|
}
|
|
else if (alignmentType == 2) {
|
|
helperStartX = centerX + minorRadius * minorX;
|
|
helperStartY = centerY + minorRadius * minorY;
|
|
helperEndX = centerX - minorRadius * minorX;
|
|
helperEndY = centerY - minorRadius * minorY;
|
|
}
|
|
else {
|
|
const double direction = alignmentType == 3 ? 1.0 : -1.0;
|
|
helperStartX = centerX + direction * focalDistance * majorX;
|
|
helperStartY = centerY + direction * focalDistance * majorY;
|
|
helperEndX = helperStartX;
|
|
helperEndY = helperStartY;
|
|
}
|
|
|
|
GCS::Ellipse ellipse;
|
|
ellipse.center = GCS::Point(¢erX, ¢erY);
|
|
ellipse.focus1 = GCS::Point(&focusX, &focusY);
|
|
ellipse.radmin = &minorRadius;
|
|
GCS::Point helperStart(&helperStartX, &helperStartY);
|
|
GCS::Point helperEnd(&helperEndX, &helperEndY);
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
double fixedFocusX = focusX;
|
|
double fixedFocusY = focusY;
|
|
double fixedMinorRadius = minorRadius;
|
|
system.addConstraintCoordinateX(ellipse.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(ellipse.center, &fixedCenterY, 2, true);
|
|
system.addConstraintCoordinateX(ellipse.focus1, &fixedFocusX, 3, true);
|
|
system.addConstraintCoordinateY(ellipse.focus1, &fixedFocusY, 4, true);
|
|
system.addConstraintEqual(ellipse.radmin, &fixedMinorRadius, 5, true);
|
|
if (alignmentType == 1) system.addConstraintInternalAlignmentEllipseMajorDiameter(ellipse, helperStart, helperEnd, 6, true);
|
|
else if (alignmentType == 2) system.addConstraintInternalAlignmentEllipseMinorDiameter(ellipse, helperStart, helperEnd, 6, true);
|
|
else if (alignmentType == 3) system.addConstraintInternalAlignmentEllipseFocus1(ellipse, helperStart, 6, true);
|
|
else system.addConstraintInternalAlignmentEllipseFocus2(ellipse, helperStart, 6, true);
|
|
GCS::VEC_pD parameters {
|
|
¢erX, ¢erY, &focusX, &focusY, &minorRadius,
|
|
&helperStartX, &helperStartY
|
|
};
|
|
if (alignmentType <= 2) {
|
|
parameters.push_back(&helperEndX);
|
|
parameters.push_back(&helperEndY);
|
|
}
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = system.calculateConstraintErrorByTag(6);
|
|
return {
|
|
static_cast<double>(status),
|
|
centerX, centerY, focusX, focusY, minorRadius,
|
|
helperStartX, helperStartY, helperEndX, helperEndY,
|
|
residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solveEllipseInternalAlignmentSet(
|
|
double centerX,
|
|
double centerY,
|
|
double majorRadius,
|
|
double minorRadius,
|
|
double rotation
|
|
)
|
|
{
|
|
if (!std::isfinite(centerX) || !std::isfinite(centerY) || !std::isfinite(rotation)
|
|
|| !(majorRadius > minorRadius) || !(minorRadius > 0.0)) {
|
|
throw std::invalid_argument("InternalAlignment requires a finite non-circular ellipse.");
|
|
}
|
|
const double focalDistance = std::sqrt(majorRadius * majorRadius - minorRadius * minorRadius);
|
|
const double majorX = std::cos(rotation);
|
|
const double majorY = std::sin(rotation);
|
|
const double minorX = -majorY;
|
|
const double minorY = majorX;
|
|
double focusX = centerX + focalDistance * majorX;
|
|
double focusY = centerY + focalDistance * majorY;
|
|
double majorStartX = centerX + majorRadius * majorX;
|
|
double majorStartY = centerY + majorRadius * majorY;
|
|
double majorEndX = centerX - majorRadius * majorX;
|
|
double majorEndY = centerY - majorRadius * majorY;
|
|
double minorStartX = centerX + minorRadius * minorX;
|
|
double minorStartY = centerY + minorRadius * minorY;
|
|
double minorEndX = centerX - minorRadius * minorX;
|
|
double minorEndY = centerY - minorRadius * minorY;
|
|
double focus1X = focusX;
|
|
double focus1Y = focusY;
|
|
double focus2X = centerX - focalDistance * majorX;
|
|
double focus2Y = centerY - focalDistance * majorY;
|
|
|
|
GCS::Ellipse ellipse;
|
|
ellipse.center = GCS::Point(¢erX, ¢erY);
|
|
ellipse.focus1 = GCS::Point(&focusX, &focusY);
|
|
ellipse.radmin = &minorRadius;
|
|
GCS::Point majorStart(&majorStartX, &majorStartY);
|
|
GCS::Point majorEnd(&majorEndX, &majorEndY);
|
|
GCS::Point minorStart(&minorStartX, &minorStartY);
|
|
GCS::Point minorEnd(&minorEndX, &minorEndY);
|
|
GCS::Point focus1(&focus1X, &focus1Y);
|
|
GCS::Point focus2(&focus2X, &focus2Y);
|
|
GCS::System system;
|
|
double fixedCenterX = centerX;
|
|
double fixedCenterY = centerY;
|
|
double fixedFocusX = focusX;
|
|
double fixedFocusY = focusY;
|
|
double fixedMinorRadius = minorRadius;
|
|
system.addConstraintCoordinateX(ellipse.center, &fixedCenterX, 1, true);
|
|
system.addConstraintCoordinateY(ellipse.center, &fixedCenterY, 2, true);
|
|
system.addConstraintCoordinateX(ellipse.focus1, &fixedFocusX, 3, true);
|
|
system.addConstraintCoordinateY(ellipse.focus1, &fixedFocusY, 4, true);
|
|
system.addConstraintEqual(ellipse.radmin, &fixedMinorRadius, 5, true);
|
|
system.addConstraintInternalAlignmentEllipseMajorDiameter(ellipse, majorStart, majorEnd, 6, true);
|
|
system.addConstraintInternalAlignmentEllipseMinorDiameter(ellipse, minorStart, minorEnd, 7, true);
|
|
system.addConstraintInternalAlignmentEllipseFocus1(ellipse, focus1, 8, true);
|
|
system.addConstraintInternalAlignmentEllipseFocus2(ellipse, focus2, 9, true);
|
|
GCS::VEC_pD parameters {
|
|
¢erX, ¢erY, &focusX, &focusY, &minorRadius,
|
|
&majorStartX, &majorStartY, &majorEndX, &majorEndY,
|
|
&minorStartX, &minorStartY, &minorEndX, &minorEndY,
|
|
&focus1X, &focus1Y, &focus2X, &focus2Y
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double majorResidual = system.calculateConstraintErrorByTag(6);
|
|
const double minorResidual = system.calculateConstraintErrorByTag(7);
|
|
const double focus1Residual = system.calculateConstraintErrorByTag(8);
|
|
const double focus2Residual = system.calculateConstraintErrorByTag(9);
|
|
return {
|
|
static_cast<double>(status),
|
|
centerX, centerY, focusX, focusY, minorRadius,
|
|
majorResidual, minorResidual, focus1Residual, focus2Residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solvePointOnCubicBspline(
|
|
double pointX,
|
|
double pointY,
|
|
double pole0X,
|
|
double pole0Y,
|
|
double pole1X,
|
|
double pole1Y,
|
|
double pole2X,
|
|
double pole2Y,
|
|
double pole3X,
|
|
double pole3Y,
|
|
double pointParameter
|
|
)
|
|
{
|
|
std::vector<double> poleX {pole0X, pole1X, pole2X, pole3X};
|
|
std::vector<double> poleY {pole0Y, pole1Y, pole2Y, pole3Y};
|
|
std::vector<GCS::Point> poles;
|
|
poles.reserve(4);
|
|
for (std::size_t index = 0; index < poleX.size(); ++index) {
|
|
poles.emplace_back(&poleX[index], &poleY[index]);
|
|
}
|
|
std::vector<double> weights(4, 1.0);
|
|
std::vector<double*> weightPointers;
|
|
weightPointers.reserve(4);
|
|
for (double& weight : weights) weightPointers.push_back(&weight);
|
|
std::vector<double> knots {0.0, 1.0};
|
|
std::vector<double*> knotPointers {&knots[0], &knots[1]};
|
|
GCS::BSpline bspline;
|
|
bspline.start = poles.front();
|
|
bspline.end = poles.back();
|
|
bspline.poles = poles;
|
|
bspline.weights = weightPointers;
|
|
bspline.knots = knotPointers;
|
|
bspline.mult = {4, 4};
|
|
bspline.degree = 3;
|
|
bspline.periodic = false;
|
|
bspline.setupFlattenedKnots();
|
|
GCS::Point point(&pointX, &pointY);
|
|
GCS::System system;
|
|
double fixedPointX = pointX;
|
|
system.addConstraintCoordinateX(point, &fixedPointX, 1, true);
|
|
system.addConstraintPointOnBSpline(point, bspline, &pointParameter, 2, true);
|
|
GCS::VEC_pD parameters {&pointX, &pointY, &pointParameter};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const GCS::DeriVector2 curvePoint = bspline.Value(pointParameter, 1.0);
|
|
const double residualX = pointX - curvePoint.x;
|
|
const double residualY = pointY - curvePoint.y;
|
|
return {static_cast<double>(status), pointX, pointY, pointParameter, residualX, residualY};
|
|
}
|
|
|
|
std::vector<double> solveCubicBsplineWeight(
|
|
double pole0X,
|
|
double pole0Y,
|
|
double pole1X,
|
|
double pole1Y,
|
|
double pole2X,
|
|
double pole2Y,
|
|
double pole3X,
|
|
double pole3Y,
|
|
double weight0,
|
|
double weight1,
|
|
double weight2,
|
|
double weight3,
|
|
int controlPointIndex,
|
|
double targetWeight
|
|
)
|
|
{
|
|
if (controlPointIndex < 0 || controlPointIndex > 3) {
|
|
throw std::invalid_argument("B-spline Weight control-point index must be in the range 0..3.");
|
|
}
|
|
if (!(weight0 > 0.0) || !(weight1 > 0.0) || !(weight2 > 0.0) || !(weight3 > 0.0)
|
|
|| !(targetWeight > 0.0) || !std::isfinite(targetWeight)) {
|
|
throw std::invalid_argument("B-spline weights must be positive and finite.");
|
|
}
|
|
std::vector<double> poleX {pole0X, pole1X, pole2X, pole3X};
|
|
std::vector<double> poleY {pole0Y, pole1Y, pole2Y, pole3Y};
|
|
std::vector<GCS::Point> poles;
|
|
poles.reserve(4);
|
|
for (std::size_t index = 0; index < poleX.size(); ++index) {
|
|
poles.emplace_back(&poleX[index], &poleY[index]);
|
|
}
|
|
std::vector<double> weights {weight0, weight1, weight2, weight3};
|
|
std::vector<double*> weightPointers;
|
|
weightPointers.reserve(4);
|
|
for (double& weight : weights) weightPointers.push_back(&weight);
|
|
std::vector<double> knots {0.0, 1.0};
|
|
std::vector<double*> knotPointers {&knots[0], &knots[1]};
|
|
GCS::BSpline bspline;
|
|
bspline.start = poles.front();
|
|
bspline.end = poles.back();
|
|
bspline.poles = poles;
|
|
bspline.weights = weightPointers;
|
|
bspline.knots = knotPointers;
|
|
bspline.mult = {4, 4};
|
|
bspline.degree = 3;
|
|
bspline.periodic = false;
|
|
bspline.setupFlattenedKnots();
|
|
double helperCenterX = poleX[controlPointIndex];
|
|
double helperCenterY = poleY[controlPointIndex];
|
|
double helperRadius = weights[controlPointIndex];
|
|
GCS::Circle helper;
|
|
helper.center = GCS::Point(&helperCenterX, &helperCenterY);
|
|
helper.rad = &helperRadius;
|
|
GCS::System system;
|
|
system.addConstraintInternalAlignmentBSplineControlPoint(bspline, helper, controlPointIndex, 1, true);
|
|
system.addConstraintCircleRadius(helper, &targetWeight, 2, true);
|
|
GCS::VEC_pD parameters {
|
|
&weights[controlPointIndex],
|
|
&helperCenterX,
|
|
&helperCenterY,
|
|
&helperRadius
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double alignmentResidual = system.calculateConstraintErrorByTag(1);
|
|
const double weightResidual = system.calculateConstraintErrorByTag(2);
|
|
return {
|
|
static_cast<double>(status),
|
|
weights[0], weights[1], weights[2], weights[3],
|
|
helperCenterX, helperCenterY, helperRadius,
|
|
alignmentResidual, weightResidual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solveLineRelation(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY,
|
|
double secondLength,
|
|
bool perpendicular
|
|
)
|
|
{
|
|
GCS::Line first;
|
|
first.p1 = GCS::Point(&firstStartX, &firstStartY);
|
|
first.p2 = GCS::Point(&firstEndX, &firstEndY);
|
|
GCS::Line second;
|
|
second.p1 = GCS::Point(&secondStartX, &secondStartY);
|
|
second.p2 = GCS::Point(&secondEndX, &secondEndY);
|
|
GCS::System system;
|
|
double fixedFirstStartX = firstStartX;
|
|
double fixedFirstStartY = firstStartY;
|
|
double fixedFirstEndX = firstEndX;
|
|
double fixedFirstEndY = firstEndY;
|
|
double fixedSecondStartX = secondStartX;
|
|
double fixedSecondStartY = secondStartY;
|
|
system.addConstraintCoordinateX(first.p1, &fixedFirstStartX, 1, true);
|
|
system.addConstraintCoordinateY(first.p1, &fixedFirstStartY, 2, true);
|
|
system.addConstraintCoordinateX(first.p2, &fixedFirstEndX, 3, true);
|
|
system.addConstraintCoordinateY(first.p2, &fixedFirstEndY, 4, true);
|
|
system.addConstraintCoordinateX(second.p1, &fixedSecondStartX, 5, true);
|
|
system.addConstraintCoordinateY(second.p1, &fixedSecondStartY, 6, true);
|
|
if (perpendicular) {
|
|
system.addConstraintPerpendicular(first, second, 7, true);
|
|
} else {
|
|
system.addConstraintParallel(first, second, 7, true);
|
|
}
|
|
system.addConstraintP2PDistance(second.p1, second.p2, &secondLength, 8, true);
|
|
GCS::VEC_pD parameters {
|
|
&firstStartX, &firstStartY, &firstEndX, &firstEndY,
|
|
&secondStartX, &secondStartY, &secondEndX, &secondEndY
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double firstDx = firstEndX - firstStartX;
|
|
const double firstDy = firstEndY - firstStartY;
|
|
const double secondDx = secondEndX - secondStartX;
|
|
const double secondDy = secondEndY - secondStartY;
|
|
const double scale = std::hypot(firstDx, firstDy) * std::hypot(secondDx, secondDy);
|
|
const double residual = scale == 0.0
|
|
? 1.0
|
|
: (perpendicular
|
|
? (firstDx * secondDx + firstDy * secondDy) / scale
|
|
: (firstDx * secondDy - firstDy * secondDx) / scale);
|
|
return {
|
|
static_cast<double>(status),
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solveParallelLines(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY,
|
|
double secondLength
|
|
)
|
|
{
|
|
return solveLineRelation(
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
secondLength, false
|
|
);
|
|
}
|
|
|
|
std::vector<double> solvePerpendicularLines(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY,
|
|
double secondLength
|
|
)
|
|
{
|
|
return solveLineRelation(
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
secondLength, true
|
|
);
|
|
}
|
|
|
|
std::vector<double> solveCoincidentLinePoints(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY,
|
|
double secondLength,
|
|
bool firstEnd,
|
|
bool secondEnd
|
|
)
|
|
{
|
|
GCS::Line first;
|
|
first.p1 = GCS::Point(&firstStartX, &firstStartY);
|
|
first.p2 = GCS::Point(&firstEndX, &firstEndY);
|
|
GCS::Line second;
|
|
second.p1 = GCS::Point(&secondStartX, &secondStartY);
|
|
second.p2 = GCS::Point(&secondEndX, &secondEndY);
|
|
GCS::System system;
|
|
double fixedFirstStartX = firstStartX;
|
|
double fixedFirstStartY = firstStartY;
|
|
double fixedFirstEndX = firstEndX;
|
|
double fixedFirstEndY = firstEndY;
|
|
system.addConstraintCoordinateX(first.p1, &fixedFirstStartX, 1, true);
|
|
system.addConstraintCoordinateY(first.p1, &fixedFirstStartY, 2, true);
|
|
system.addConstraintCoordinateX(first.p2, &fixedFirstEndX, 3, true);
|
|
system.addConstraintCoordinateY(first.p2, &fixedFirstEndY, 4, true);
|
|
GCS::Point& firstPoint = firstEnd ? first.p2 : first.p1;
|
|
GCS::Point& secondPoint = secondEnd ? second.p2 : second.p1;
|
|
system.addConstraintP2PCoincident(firstPoint, secondPoint, 5, true);
|
|
system.addConstraintP2PDistance(second.p1, second.p2, &secondLength, 6, true);
|
|
GCS::VEC_pD parameters {
|
|
&firstStartX, &firstStartY, &firstEndX, &firstEndY,
|
|
&secondStartX, &secondStartY, &secondEndX, &secondEndY
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double selectedFirstX = firstEnd ? firstEndX : firstStartX;
|
|
const double selectedFirstY = firstEnd ? firstEndY : firstStartY;
|
|
const double selectedSecondX = secondEnd ? secondEndX : secondStartX;
|
|
const double selectedSecondY = secondEnd ? secondEndY : secondStartY;
|
|
const double residual = std::hypot(selectedFirstX - selectedSecondX, selectedFirstY - selectedSecondY);
|
|
return {
|
|
static_cast<double>(status),
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
residual
|
|
};
|
|
}
|
|
|
|
std::vector<double> solveCoincidentLines(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY,
|
|
double secondLength
|
|
)
|
|
{
|
|
return solveCoincidentLinePoints(
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
secondLength, true, false
|
|
);
|
|
}
|
|
|
|
std::vector<double> solveSnellsLawLines(
|
|
double firstStartX,
|
|
double firstStartY,
|
|
double firstEndX,
|
|
double firstEndY,
|
|
double secondStartX,
|
|
double secondStartY,
|
|
double secondEndX,
|
|
double secondEndY,
|
|
double boundaryStartX,
|
|
double boundaryStartY,
|
|
double boundaryEndX,
|
|
double boundaryEndY,
|
|
double refractiveRatio,
|
|
bool firstEnd,
|
|
bool secondEnd
|
|
)
|
|
{
|
|
const double firstLength = std::hypot(firstEndX - firstStartX, firstEndY - firstStartY);
|
|
double secondLength = std::hypot(secondEndX - secondStartX, secondEndY - secondStartY);
|
|
const double boundaryLength = std::hypot(boundaryEndX - boundaryStartX, boundaryEndY - boundaryStartY);
|
|
if (!(firstLength > 0.0) || !(secondLength > 0.0) || !(boundaryLength > 0.0)) {
|
|
throw std::invalid_argument("SnellsLaw requires three non-degenerate lines.");
|
|
}
|
|
if (!std::isfinite(refractiveRatio) || !(refractiveRatio > 0.0)) {
|
|
throw std::invalid_argument("SnellsLaw refractive ratio must be positive and finite.");
|
|
}
|
|
|
|
GCS::Line first;
|
|
first.p1 = GCS::Point(&firstStartX, &firstStartY);
|
|
first.p2 = GCS::Point(&firstEndX, &firstEndY);
|
|
GCS::Line second;
|
|
second.p1 = GCS::Point(&secondStartX, &secondStartY);
|
|
second.p2 = GCS::Point(&secondEndX, &secondEndY);
|
|
GCS::Line boundary;
|
|
boundary.p1 = GCS::Point(&boundaryStartX, &boundaryStartY);
|
|
boundary.p2 = GCS::Point(&boundaryEndX, &boundaryEndY);
|
|
GCS::Point& firstPoint = firstEnd ? first.p2 : first.p1;
|
|
GCS::Point& secondPoint = secondEnd ? second.p2 : second.p1;
|
|
|
|
const double junctionDistance = std::hypot(*firstPoint.x - *secondPoint.x, *firstPoint.y - *secondPoint.y);
|
|
const double boundaryDistance = std::abs(
|
|
(*firstPoint.x - boundaryStartX) * (boundaryEndY - boundaryStartY)
|
|
- (*firstPoint.y - boundaryStartY) * (boundaryEndX - boundaryStartX)
|
|
) / boundaryLength;
|
|
if (junctionDistance > 1e-7 || boundaryDistance > 1e-7) {
|
|
throw std::invalid_argument("SnellsLaw ray endpoints must coincide on the boundary.");
|
|
}
|
|
|
|
double n1 = refractiveRatio;
|
|
double n2 = 1.0;
|
|
if (std::abs(refractiveRatio) >= 1.0) {
|
|
n1 = 1.0;
|
|
n2 = refractiveRatio;
|
|
} else {
|
|
n1 = 1.0 / refractiveRatio;
|
|
n2 = 1.0;
|
|
}
|
|
|
|
GCS::System system;
|
|
double fixedFirstStartX = firstStartX;
|
|
double fixedFirstStartY = firstStartY;
|
|
double fixedFirstEndX = firstEndX;
|
|
double fixedFirstEndY = firstEndY;
|
|
double fixedBoundaryStartX = boundaryStartX;
|
|
double fixedBoundaryStartY = boundaryStartY;
|
|
double fixedBoundaryEndX = boundaryEndX;
|
|
double fixedBoundaryEndY = boundaryEndY;
|
|
system.addConstraintCoordinateX(first.p1, &fixedFirstStartX, 1, true);
|
|
system.addConstraintCoordinateY(first.p1, &fixedFirstStartY, 2, true);
|
|
system.addConstraintCoordinateX(first.p2, &fixedFirstEndX, 3, true);
|
|
system.addConstraintCoordinateY(first.p2, &fixedFirstEndY, 4, true);
|
|
system.addConstraintCoordinateX(boundary.p1, &fixedBoundaryStartX, 5, true);
|
|
system.addConstraintCoordinateY(boundary.p1, &fixedBoundaryStartY, 6, true);
|
|
system.addConstraintCoordinateX(boundary.p2, &fixedBoundaryEndX, 7, true);
|
|
system.addConstraintCoordinateY(boundary.p2, &fixedBoundaryEndY, 8, true);
|
|
system.addConstraintP2PCoincident(firstPoint, secondPoint, 9, true);
|
|
system.addConstraintP2PDistance(second.p1, second.p2, &secondLength, 10, true);
|
|
system.addConstraintSnellsLaw(
|
|
first,
|
|
second,
|
|
boundary,
|
|
firstPoint,
|
|
&n1,
|
|
&n2,
|
|
!firstEnd,
|
|
secondEnd,
|
|
11,
|
|
true
|
|
);
|
|
GCS::VEC_pD parameters {
|
|
&firstStartX, &firstStartY, &firstEndX, &firstEndY,
|
|
&secondStartX, &secondStartY, &secondEndX, &secondEndY,
|
|
&boundaryStartX, &boundaryStartY, &boundaryEndX, &boundaryEndY
|
|
};
|
|
const int status = system.solve(parameters, true, GCS::DogLeg, false);
|
|
if (status <= GCS::Converged) {
|
|
system.applySolution();
|
|
}
|
|
const double residual = system.calculateConstraintErrorByTag(11);
|
|
return {
|
|
static_cast<double>(status),
|
|
firstStartX, firstStartY, firstEndX, firstEndY,
|
|
secondStartX, secondStartY, secondEndX, secondEndY,
|
|
boundaryStartX, boundaryStartY, boundaryEndX, boundaryEndY,
|
|
residual
|
|
};
|
|
}
|
|
} // namespace
|
|
|
|
EMSCRIPTEN_BINDINGS(web_freecad_planegcs)
|
|
{
|
|
emscripten::register_vector<double>("DoubleVector");
|
|
emscripten::function("solveHorizontalDistance", &solveHorizontalDistance);
|
|
emscripten::function("solveVerticalDistance", &solveVerticalDistance);
|
|
emscripten::function("solveDistanceX", &solveDistanceX);
|
|
emscripten::function("solveDistanceY", &solveDistanceY);
|
|
emscripten::function("solveAngle", &solveAngle);
|
|
emscripten::function("solveCircleRadius", &solveCircleRadius);
|
|
emscripten::function("solveCircleDiameter", &solveCircleDiameter);
|
|
emscripten::function("solveEqualLines", &solveEqualLines);
|
|
emscripten::function("solveEqualCircles", &solveEqualCircles);
|
|
emscripten::function("solveTangentCircles", &solveTangentCircles);
|
|
emscripten::function("solvePointSymmetry", &solvePointSymmetry);
|
|
emscripten::function("solvePointOnLine", &solvePointOnLine);
|
|
emscripten::function("solvePointOnCircle", &solvePointOnCircle);
|
|
emscripten::function("solvePointOnArc", &solvePointOnArc);
|
|
emscripten::function("solvePointOnEllipse", &solvePointOnEllipse);
|
|
emscripten::function("solveEllipseInternalAlignment", &solveEllipseInternalAlignment);
|
|
emscripten::function("solveEllipseInternalAlignmentSet", &solveEllipseInternalAlignmentSet);
|
|
emscripten::function("solvePointOnCubicBspline", &solvePointOnCubicBspline);
|
|
emscripten::function("solveCubicBsplineWeight", &solveCubicBsplineWeight);
|
|
emscripten::function("solveParallelLines", &solveParallelLines);
|
|
emscripten::function("solvePerpendicularLines", &solvePerpendicularLines);
|
|
emscripten::function("solveCoincidentLines", &solveCoincidentLines);
|
|
emscripten::function("solveCoincidentLinePoints", &solveCoincidentLinePoints);
|
|
emscripten::function("solveSnellsLawLines", &solveSnellsLawLines);
|
|
}
|