void circle(t_bunny_position *pos, int r, t_data *data)
{
int x = 0;
int y = r;
int d = r - 1;
while (y >= x)
{
circle2(data, pos, x, y);
if (d >= 2 * x)
{
d = d - 2 * x - 1;
x = x + 1;
}
else if (d < 2 * (r - y))
{
d = d + 2 * y - 1;
y = y - 1;
}
else
{
d = d + 2 * (y - x - 1);
y = y - 1;
x = x + 1;
}
}
}
double ringofcircles (double x0, double y0, double radius, int ncirc)
{
double delta;
double theta;
double r2;
double x, y;
int i;
delta = (M_PI * 2.0) / (double)ncirc;
/* Compute radius as if centres are on circumference of 'radius' */
r2 = radius * sin (delta);
/* Increase radius by radius of small circles */
radius += r2;
/* Recompute radius of smaller circles */
r2 = radius * sin (delta);
for (i = 0; i < ncirc; i++) {
theta = (double)i * delta;
x = radius * cos (theta);
y = radius * sin (theta);
circle2 (x0 + x, y0 + y, r2, 10.0);
}
return (radius + r2);
}
void planner_t::visualize_nodes(int image_counter)
{
/*
char buff[PATH_MAX];
//getcwd( buff, PATH_MAX );
ssize_t len = ::readlink(-d "sparseRRT", buff, sizeof(buff)-1);
if (len != -1)
{
buff[len] = '\0';
}
std::cout << buff;
*/
//system("beep");
//std::stringstream s;
//std::cout <<"Path: " <<s.str();
//params::path_file<<"/nodes_"<<image_counter<<".svg";
std::string dir;
if (params::planner == "rrt")
{
dir = params::path_file + "/nodes_rrt.svg";
}
else
{
dir = params::path_file + "/nodes_sst.svg";
}
//std::cout <<"Path: " <<dir;
svg::Dimensions dimensions(params::image_width, params::image_height);
svg::Document doc(dir, svg::Layout(dimensions, svg::Layout::BottomLeft));
//-------------------
sorted_nodes.clear();
get_max_cost();
sort(sorted_nodes);
for(unsigned i=sorted_nodes.size()-1;i!=0;i--)
{
visualize_node(sorted_nodes[i],doc,dimensions);
}
//------------------
svg::Circle circle(system->visualize_point(start_state,dimensions),params::solution_node_diameter,svg::Fill( svg::Color(255,0,0) ));
doc<<circle;
svg::Circle circle2(system->visualize_point(goal_state,dimensions),params::solution_node_diameter,svg::Fill( svg::Color(0,255,0) ));
doc<<circle2;
visualize_solution_nodes(doc,dimensions);
system->visualize_obstacles(doc,dimensions);
doc.save();
}
void
Circle2DTest::testIntersectionCircle()
{
xpcc::Circle2D<int16_t> circle1(
xpcc::Vector2i(),
10);
xpcc::Circle2D<int16_t> circle2(
xpcc::Vector2i(30, 0),
20);
xpcc::PointSet2D<int16_t> points;
// circle touch each other with one point
TEST_ASSERT_TRUE(circle1.getIntersections(circle2, points));
TEST_ASSERT_EQUALS(points.getNumberOfPoints(), 1U);
TEST_ASSERT_EQUALS(points[0].getX(), 10);
TEST_ASSERT_EQUALS(points[0].getY(), 0);
points.removeAll();
// no intersection
circle2.setRadius(10);
TEST_ASSERT_FALSE(circle1.getIntersections(circle2, points));
TEST_ASSERT_EQUALS(points.getNumberOfPoints(), 0U);
// 2 intersection points (15, -25.98) and (15, 25.98)
circle1.setRadius(30);
circle2.setRadius(30);
TEST_ASSERT_TRUE(circle1.getIntersections(circle2, points));
TEST_ASSERT_EQUALS(points.getNumberOfPoints(), 2U);
TEST_ASSERT_EQUALS(points[0].getX(), 15);
TEST_ASSERT_EQUALS(points[0].getY(), -26);
TEST_ASSERT_EQUALS(points[1].getX(), 15);
TEST_ASSERT_EQUALS(points[1].getY(), 26);
points.removeAll();
// circle 1 is contained inside circle 2
circle1.setRadius(10);
circle2.setCenter(xpcc::Vector2i(0, 0));
TEST_ASSERT_FALSE(circle1.getIntersections(circle2, points));
TEST_ASSERT_EQUALS(points.getNumberOfPoints(), 0U);
}
void display()
{
glColor3f(1, 1, 1);
glClear(GL_COLOR_BUFFER_BIT);
// glTranslatef(2,0,0);
if (flag == 0)
{
circle(r, cx, 250);
if (int(cx) >= 100 && int(cx) <= 150)
circle(30, 100, 250);
if (int(cx) >= 200 && int(cx) <= 250)
circle(30, 200, 250);
if (int(cx) >= 300 && int(cx) <= 350)
circle(30, 300, 250);
if (int(cx) >= 400 && int(cx) <= 450)
circle(30, 400, 250);
}
if (flag == 1)
circle2(60, cx, 250);
if (bloodflag == 1)
{
blood(yy1--);
}
glFlush();
cx += d;
if (cx >= 700)
{
d = -d1;
flag = 1;
}
if (cx <= 400 && flag == 1)
{
d = 0;
}
glutPostRedisplay();
}
QList<RVector> RShape::getIntersectionPointsEE(const REllipse& ellipse1, const REllipse& ellipse2) {
QList<RVector> ret;
// two full ellipses:
// if bounding boxes don't intersect, ellipses don't either:
if (ellipse1.isFullEllipse() && ellipse2.isFullEllipse() &&
!ellipse1.getBoundingBox().intersects(ellipse2.getBoundingBox())) {
return ret;
}
// normalize ellipse ratios:
REllipse ellipse1Copy = ellipse1;
if (ellipse1Copy.getMajorRadius() < ellipse1Copy.getMinorRadius()) {
ellipse1Copy.switchMajorMinor();
}
REllipse ellipse2Copy = ellipse2;
if (ellipse2Copy.getMajorRadius() < ellipse2Copy.getMinorRadius()) {
ellipse2Copy.switchMajorMinor();
}
// for later comparison, make sure that major points are in
// quadrant I or II (relative to the ellipse center):
if (fabs(ellipse1Copy.getMajorPoint().y)<RS::PointTolerance) {
if (ellipse1Copy.getMajorPoint().x<0.0) {
ellipse1Copy.setMajorPoint(-ellipse1Copy.getMajorPoint());
}
}
else {
if (ellipse1Copy.getMajorPoint().y<0.0) {
ellipse1Copy.setMajorPoint(-ellipse1Copy.getMajorPoint());
}
}
if (fabs(ellipse2Copy.getMajorPoint().y)<RS::PointTolerance) {
if (ellipse2Copy.getMajorPoint().x<0.0) {
ellipse2Copy.setMajorPoint(-ellipse2Copy.getMajorPoint());
}
}
else {
if (ellipse2Copy.getMajorPoint().y<0.0) {
ellipse2Copy.setMajorPoint(-ellipse2Copy.getMajorPoint());
}
}
// for comparison:
bool identicalCenter =
(ellipse1Copy.getCenter() - ellipse2Copy.getCenter()).getMagnitude() < RS::PointTolerance;
bool identicalShape =
(ellipse1Copy.getMajorPoint() - ellipse2Copy.getMajorPoint()).getMagnitude() < RS::PointTolerance &&
fabs(ellipse1Copy.getMajorRadius() - ellipse2Copy.getMajorRadius()) < RS::PointTolerance &&
fabs(ellipse1Copy.getMinorRadius() - ellipse2Copy.getMinorRadius()) < RS::PointTolerance;
// ellipses are identical (no intersection points):
if (identicalCenter && identicalShape) {
//qDebug() << "RShape::getIntersectionPointsEE: identical";
return ret;
}
// special case: ellipse shapes are identical (different positions):
if (identicalShape) {
double angle = -ellipse1Copy.getAngle();
double yScale = 1.0 / ellipse1Copy.getRatio();
RVector circleCenter1 = ellipse1Copy.getCenter();
circleCenter1.rotate(angle);
circleCenter1.scale(RVector(1.0, yScale));
RVector circleCenter2 = ellipse2Copy.getCenter();
circleCenter2.rotate(angle);
circleCenter2.scale(RVector(1.0, yScale));
RCircle circle1(circleCenter1, ellipse1Copy.getMajorRadius());
RCircle circle2(circleCenter2, ellipse2Copy.getMajorRadius());
ret = getIntersectionPointsCC(circle1, circle2);
RVector::scaleList(ret, RVector(1.0, 1.0/yScale));
RVector::rotateList(ret, -angle);
return ret;
}
// transform ellipse2 to coordinate system of ellipse1:
RVector centerOffset = -ellipse1Copy.getCenter();
double angleOffset = -ellipse1Copy.getAngle();
double majorRadius1 = ellipse1Copy.getMajorRadius();
double majorRadius2 = ellipse2Copy.getMajorRadius();
// special case: treat first ellipse as a line:
if (ellipse1Copy.getMinorRadius() < RS::PointTolerance ||
ellipse1Copy.getRatio() < RS::PointTolerance) {
ellipse2Copy.move(centerOffset);
ellipse2Copy.rotate(angleOffset);
RLine line(RVector(-majorRadius1,0.0),RVector(majorRadius1,0.0));
ret = getIntersectionPointsLE(line, ellipse2Copy);
RVector::rotateList(ret, -angleOffset);
RVector::moveList(ret, -centerOffset);
// qDebug() << "RShape::getIntersectionPointsEE: ellipse 1 as line";
return ret;
}
// special case: treat second ellipse as a line:
if (ellipse2Copy.getMinorRadius() < RS::PointTolerance ||
ellipse2Copy.getRatio()< RS::PointTolerance) {
ellipse2Copy.move(centerOffset);
ellipse2Copy.rotate(angleOffset);
RLine line(RVector(-majorRadius2,0.),RVector(majorRadius2,0.));
line.rotate(ellipse2Copy.getAngle(), RVector(0.,0.));
line.move(ellipse2Copy.getCenter());
ret = getIntersectionPointsLE(line, ellipse1Copy);
RVector::rotateList(ret, -angleOffset);
RVector::moveList(ret, -centerOffset);
// qDebug() << "RShape::getIntersectionPointsEE: ellipse 2 as line";
return ret;
}
double phi_1 = ellipse1Copy.getAngle();
double a1 = ellipse1Copy.getMajorRadius();
double b1 = ellipse1Copy.getMinorRadius();
double h1 = ellipse1Copy.getCenter().x;
double k1 = ellipse1Copy.getCenter().y;
double phi_2 = ellipse2Copy.getAngle();
double a2 = ellipse2Copy.getMajorRadius();
double b2 = ellipse2Copy.getMinorRadius();
double h2 = ellipse2Copy.getCenter().x;
double k2 = ellipse2Copy.getCenter().y;
int i, j, k, nroots, nychk, nintpts;
double AA, BB, CC, DD, EE, FF, H2_TR, K2_TR, A22, B22, PHI_2R;
double cosphi, cosphi2, sinphi, sinphi2, cosphisinphi;
double tmp0, tmp1, tmp2, tmp3;
double cy[5] = {0.0};
double py[5] = {0.0};
double r[3][5] = { {0.0} };
double x1, x2;
double ychk[5] = {0.0};
double xint[5];
double yint[5];
// each of the ellipse axis lengths must be positive
if ( (!(a1 > 0.0) || !(b1 > 0.0)) || (!(a2 > 0.0) || !(b2 > 0.0)) ) {
//(*rtnCode) = ERROR_ELLIPSE_PARAMETERS;
// qDebug() << "negative axis";
return QList<RVector>();
}
// the rotation angles should be between -2pi and 2pi (?)
if (fabs(phi_1) > twopi) {
phi_1 = fmod(phi_1, twopi);
}
if (fabs(phi_2) > twopi) {
phi_2 = fmod(phi_2, twopi);
}
// determine the two ellipse equations from input parameters:
// Finding the points of intersection between two general ellipses
// requires solving a quartic equation. Before attempting to solve the
// quartic, several quick tests can be used to eliminate some cases
// where the ellipses do not intersect. Optionally, can whittle away
// at the problem, by addressing the easiest cases first.
// Working with the translated+rotated ellipses simplifies the
// calculations. The ellipses are translated then rotated so that the
// first ellipse is centered at the origin and oriented with the
// coordinate axes. Then, the first ellipse will have the implicit
// (polynomial) form of
// x^2/A1^2 + y+2/B1^2 = 1
// For the second ellipse, the center is first translated by the amount
// required to put the first ellipse at the origin, e.g., by (-H1, -K1)
// Then, the center of the second ellipse is rotated by the amount
// required to orient the first ellipse with the coordinate axes, e.g.,
// through the angle -PHI_1.
// The translated and rotated center point coordinates for the second
// ellipse are found with the rotation matrix, derivations are
// described in the reference.
cosphi = cos(phi_1);
sinphi = sin(phi_1);
H2_TR = (h2 - h1)*cosphi + (k2 - k1)*sinphi;
K2_TR = (h1 - h2)*sinphi + (k2 - k1)*cosphi;
PHI_2R = phi_2 - phi_1;
if (fabs(PHI_2R) > twopi) {
PHI_2R = fmod(PHI_2R, twopi);
}
// Calculate implicit (Polynomial) coefficients for the second ellipse
// in its translated-by (-H1, -H2) and rotated-by -PHI_1 postion
// AA*x^2 + BB*x*y + CC*y^2 + DD*x + EE*y + FF = 0
// Formulas derived in the reference
// To speed things up, store multiply-used expressions first
cosphi = cos(PHI_2R);
cosphi2 = cosphi*cosphi;
sinphi = sin(PHI_2R);
sinphi2 = sinphi*sinphi;
cosphisinphi = 2.0*cosphi*sinphi;
A22 = a2*a2;
B22 = b2*b2;
tmp0 = (cosphi*H2_TR + sinphi*K2_TR)/A22;
tmp1 = (sinphi*H2_TR - cosphi*K2_TR)/B22;
tmp2 = cosphi*H2_TR + sinphi*K2_TR;
tmp3 = sinphi*H2_TR - cosphi*K2_TR;
// implicit polynomial coefficients for the second ellipse
AA = cosphi2/A22 + sinphi2/B22;
BB = cosphisinphi/A22 - cosphisinphi/B22;
CC = sinphi2/A22 + cosphi2/B22;
DD = -2.0*cosphi*tmp0 - 2.0*sinphi*tmp1;
EE = -2.0*sinphi*tmp0 + 2.0*cosphi*tmp1;
FF = tmp2*tmp2/A22 + tmp3*tmp3/B22 - 1.0;
// qDebug() << "second ellipse:";
// qDebug() << "AA: " << AA;
// qDebug() << "BB: " << BB;
// qDebug() << "CC: " << CC;
// qDebug() << "DD: " << DD;
// qDebug() << "EE: " << EE;
// qDebug() << "FF: " << FF;
// create and solve the quartic equation to find intersection points:
// If execution arrives here, the ellipses are at least 'close' to
// intersecting.
// Coefficients for the Quartic Polynomial in y are calculated from
// the two implicit equations.
// Formulas for these coefficients are derived in the reference.
cy[4] = pow(a1, 4.0)*AA*AA + b1*b1*(a1*a1*(BB*BB - 2.0*AA*CC) + b1*b1*CC*CC);
cy[3] = 2.0*b1*(b1*b1*CC*EE + a1*a1*(BB*DD - AA*EE));
cy[2] = a1*a1*((b1*b1*(2.0*AA*CC - BB*BB) + DD*DD - 2.0*AA*FF) - 2.0*a1*a1*AA*AA) + b1*b1*(2.0*CC*FF + EE*EE);
cy[1] = 2.0*b1*(a1*a1*(AA*EE - BB*DD) + EE*FF);
cy[0] = (a1*(a1*AA - DD) + FF)*(a1*(a1*AA + DD) + FF);
// Once the coefficients for the Quartic Equation in y are known, the
// roots of the quartic polynomial will represent y-values of the
// intersection points of the two ellipse curves.
// The quartic sometimes degenerates into a polynomial of lesser
// degree, so handle all possible cases.
if (fabs (cy[4]) > 0.0) {
// quartic coefficient nonzero, use quartic formula:
for (i = 0; i <= 3; i++) {
py[4-i] = cy[i]/cy[4];
}
py[0] = 1.0;
RMath::getBiQuadRoots (py, r);
nroots = 4;
}
else if (fabs (cy[3]) > 0.0) {
// quartic degenerates to cubic, use cubic formula:
for (i = 0; i <= 2; i++) {
py[3-i] = cy[i]/cy[3];
}
py[0] = 1.0;
RMath::getCubicRoots (py, r);
nroots = 3;
}
else if (fabs (cy[2]) > 0.0)
{
// quartic degenerates to quadratic, use quadratic formula:
for (i = 0; i <= 1; i++) {
py[2-i] = cy[i]/cy[2];
}
py[0] = 1.0;
RMath::getQuadRoots(py, r);
nroots = 2;
}
else if (fabs (cy[1]) > 0.0) {
// quartic degenerates to linear: solve directly:
// cy[1]*Y + cy[0] = 0
r[1][1] = (-cy[0]/cy[1]);
r[2][1] = 0.0;
nroots = 1;
}
else
{
// completely degenerate quartic: ellipses identical?
// a completely degenerate quartic, which would seem to
// indicate that the ellipses are identical. However, some
// configurations lead to a degenerate quartic with no
// points of intersection.
nroots = 0;
}
//qDebug() << "nroots" << nroots;
// check roots of the quartic: are they points of intersection?
// determine which roots are real, discard any complex roots
nychk = 0;
for (i = 1; i <= nroots; i++) {
if (fabs (r[2][i]) < epsTolerance) {
nychk++;
ychk[nychk] = r[1][i]*b1;
//qDebug() << "real root: ychk[nychk]: " << ychk[nychk];
}
}
// sort the real roots by straight insertion
for (j = 2; j <= nychk; j++) {
tmp0 = ychk[j];
for (k = j - 1; k >= 1; k--) {
if (ychk[k] <= tmp0) {
break;
}
ychk[k+1] = ychk[k];
}
ychk[k+1] = tmp0;
}
// determine whether polynomial roots are points of intersection
// for the two ellipses
nintpts = 0;
for (i = 1; i <= nychk; i++) {
// check for multiple roots
if ((i > 1) && (fabs(ychk[i] - ychk[i-1]) < (epsTolerance/2.0))) {
continue;
}
// check intersection points for ychk[i]
if (fabs(ychk[i]) > b1) {
x1 = 0.0;
}
else {
x1 = a1*sqrt (1.0 - (ychk[i]*ychk[i])/(b1*b1));
}
x2 = -x1;
// qDebug() << "fabs(ellipse2tr(x1, ychk[i], AA, BB, CC, DD, EE, FF)): " << fabs(ellipse2tr(x1, ychk[i], AA, BB, CC, DD, EE, FF));
if (fabs(ellipse2tr(x1, ychk[i], AA, BB, CC, DD, EE, FF)) < epsTolerance/2.0) {
// qDebug() << "got intersection I...";
nintpts++;
if (nintpts > 4) {
//(*rtnCode) = ERROR_INTERSECTION_PTS;
return QList<RVector>();
}
xint[nintpts] = x1;
yint[nintpts] = ychk[i];
// qDebug() << "intersection I: " << xint[nintpts] << "/" << yint[nintpts];
}
// qDebug() << "fabs(ellipse2tr(x2, ychk[i], AA, BB, CC, DD, EE, FF))" << fabs(ellipse2tr(x2, ychk[i], AA, BB, CC, DD, EE, FF));
if ((fabs(ellipse2tr(x2, ychk[i], AA, BB, CC, DD, EE, FF)) < epsTolerance/2.0) && (fabs(x2 - x1) > epsTolerance/2.0)) {
// qDebug() << "got intersection II...";
nintpts++;
if (nintpts > 4) {
//(*rtnCode) = ERROR_INTERSECTION_PTS;
return QList<RVector>();
}
xint[nintpts] = x2;
yint[nintpts] = ychk[i];
// qDebug() << "intersection II: " << xint[nintpts] << "/" << yint[nintpts];
}
}
//qDebug() << "nintpts: " << nintpts;
for (int i=1; i<=nintpts; i++) {
// qDebug() << "intersection: x/y: " << xint[i] << "/" << yint[i];
RVector v(xint[i], yint[i]);
v.rotate(-angleOffset);
v.move(-centerOffset);
ret.append(v);
}
return ret;
}
