ARM seems fine, based on frame1

geometry/Arc.cpp

@@ -14,6 +14,7 @@
 #include "Circle.h"
 #include "Point.h"
 #include "ARMBuilder.h"
+#include "Vector.h"
 
 Arc::Arc(const Configuration& start, const Point& end, bool shorter) {
     Point p = start.inverseTransform(end);
@@ -65,7 +66,7 @@ bool Arc::isIntersect(const Segment& seg) const {
         return path.isIntersect(seg);
     } else {
         /* Circular segment */
-        Point center(start.position.x - radius * sin(start.orientation), start.position.y + radius * cos(start.orientation));
+        Point center(getCenter());
         Circle circle(center, radius);
         pointList intersections = circle.getIntersections(Line(seg));
         bool intersect = false;
@@ -94,11 +95,38 @@ bool Arc::isIntersect(const Segment& seg) const {
  * @param pos Relative to absolute frame.
  * @return Translated arc
  */
-Arc Arc::translate(const Point& pos) {
-    Arc a(*this);
-    a.start.position = a.start.transform(pos);
-    a.end.position = a.end.transform(pos);
-    return a;
+Arc& Arc::translateToRobotCornerArc(const Point& pos) {
+    if (std::isinf(radius)) {
+        start.position = start.transform(pos);
+        end.position = end.transform(pos);
+    } else {
+        Point center = getCenter();
+        start.position = start.transform(pos);
+        Vector r(start.position - center);
+        start.orientation = wrapAngle(r.getFi() + sgn(radius) * M_PI_2);
+        end.position = end.transform(pos);
+        radius = sgn(radius) * r.getLength();
+    }
+    return *this;
+}
+
+Arc& Arc::translateToObstacleCornerArc(const Point& pos) {
+    if (std::isinf(radius)) {
+        end.position = pos - end.position + start.position;
+        start.position = pos;
+        dTheta = -dTheta;
+    } else {
+        Point center = getCenter();
+        Vector r(pos - center);
+        start.position = pos;
+        start.orientation = wrapAngle(r.getFi() + sgn(radius) * M_PI_2);
+        radius = sgn(radius) * r.getLength();
+        dTheta = -dTheta;
+        end.position.x = center.x + fabs(radius) * cos(start.orientation - sgn(radius)*M_PI_2 + dTheta);
+        end.position.y = center.y + fabs(radius) * sin(start.orientation - sgn(radius)*M_PI_2 + dTheta);
+        end.orientation = wrapAngle(start.orientation + dTheta);
+    }
+    return *this;
 }
 
 Configuration& Arc::getEndConfig() {
@@ -125,3 +153,7 @@ std::ostream& operator<<(std::ostream& os, Arc& arc) {
     os << arc.getStartConfig() << ";" << arc.getEndConfig() << ";" << arc.getRadius() << ";" << arc.getDTheta() << ";" << arc.getDirection();
     return os;
 }
+
+Point Arc::getCenter() const {
+    return Point(start.position.x - radius * sin(start.orientation), start.position.y + radius * cos(start.orientation));
+}

geometry/Arc.h

@@ -17,12 +17,14 @@ public:
     
     double getRadius();
     bool isIntersect(const Segment& seg) const;
-    Arc translate(const Point& pos);
+    Arc& translateToRobotCornerArc(const Point& pos);
+    Arc& translateToObstacleCornerArc(const Point& pos);
     Configuration& getEndConfig();
     Configuration& getStartConfig();
     double getDistance();
     double getDTheta();
     bool getDirection();
+    Point getCenter() const;
 private:
     Configuration start;
     Configuration end;

geometry/Shape.cpp

@@ -14,7 +14,7 @@ void Shape::setPoints(pointList& list) {
     points = list;
 }
 
-const pointList& Shape::getPoints() {
+pointList& Shape::getPoints() {
     return points;
 }
 
@@ -72,3 +72,10 @@ bool Shape::isIntersect(const Arc& arc) {
     }
     return intersect;
 }
+
+Shape& Shape::transform(Configuration& conf) {
+    for(unsigned i = 0; i<points.size(); i++) {
+        points[i] = conf.transform(points[i]);
+    }
+    return *this;
+}

geometry/Shape.h

@@ -10,13 +10,14 @@ class Shape
 public:
 	Shape();
 	Shape(pointList& list);
-	const pointList& getPoints();
+	pointList& getPoints();
 	const Point& getPoint(int index);
 	int getNumberOfPoints();
 	Point getInsidePoint();
 	bool isInside(const Point& p);
         double getMinDistance(const Segment& segment);
         bool isIntersect(const Arc& arc);
+        Shape& transform(Configuration& conf);
 protected:
 	void setPoints(pointList& list);
         pointList points;	

geometry/Vector.cpp

@@ -5,6 +5,8 @@
  * Created on 2014. április 10., 15:32
  */
 
+#include <math.h>
+
 #include "Vector.h"
 
 Vector::Vector(const Point& p)
@@ -15,6 +17,14 @@ Vector::Vector(double x, double y)
 : Point(x,y) {
 }
 
+Vector::Vector(const Point& p1, const Point& p2)
+: Point(p2-p1){
+}
+
+Vector::Vector(const Segment& seg)
+: Point(seg.getB()-seg.getA()) {
+}
+
 double Vector::getScalarProduct(const Vector& v) {
     return x * v.x + y * v.y;
 }
@@ -22,3 +32,11 @@ double Vector::getScalarProduct(const Vector& v) {
 double Vector::operator *(const Vector& rhs) {
     return getScalarProduct(rhs);
 }
+
+double Vector::getFi() const {
+    return atan2(y,x);
+}
+
+double Vector::getLength() const {
+    return sqrt(x*x+y*y);
+}

geometry/Vector.h

@@ -9,14 +9,19 @@
 #define	VECTOR_H
 
 #include "Point.h"
+#include "Segment.h"
 
 class Vector : public Point {
 public:
     Vector(double x, double y);
     Vector(const Point& p);
+    Vector(const Point& p1, const Point& p2);
+    Vector(const Segment& seg);
 
     double getScalarProduct(const Vector& v);
     double operator*(const Vector& rhs);
+    double getFi() const;
+    double getLength() const;
 private:
 };
 

planner/Environment.cpp

@@ -56,5 +56,18 @@ bool Environment::isCollisionFree(const Arc& arc) {
         collision |= obstacles[i].isIntersect(arc);
     }
     obstacles.pop_back();
-    return collision;
+    return !collision;
+}
+
+pointList Environment::getAllObstaclePoints() {
+    pointList points;
+    obstacles.push_back(boundary);
+    for(unsigned i = 0; i<obstacles.size(); i++) {
+        pointList& obsPoints = obstacles[i].getPoints();
+        for (unsigned j = 0; j < obsPoints.size(); j++) {
+            points.push_back(obsPoints[j]);
+        }
+    }
+    obstacles.pop_back();
+    return points;
 }

planner/Environment.h

@@ -19,6 +19,7 @@ public:
         double getHeight();
         bool isInsideObstacle(const Point& p);
         bool isCollisionFree(const Arc& arc);
+        pointList getAllObstaclePoints();
 private:
 	shapeList obstacles;
 	Rectangle boundary;

planner/Scene.cpp

@@ -20,9 +20,20 @@ Scene::Scene(Environment& env, Robot& robot)
  */
 bool Scene::isAdmissible(Arc& arc) {
     bool admissible = true;
-    pointList robotPoints = robot.getBody().getPoints();
+    Shape robotBody = robot.getBody();
+    pointList robotPoints = robotBody.getPoints();
     for (unsigned i = 0; i < robotPoints.size() && admissible; i++) {
-        admissible &= !env.isCollisionFree(arc.translate(robotPoints[i]));
+        Arc movedArc(arc);
+        admissible &= env.isCollisionFree(movedArc.translateToRobotCornerArc(robotPoints[i]));
+    }
+
+    if (admissible) {
+        robotBody.transform(arc.getStartConfig());
+        pointList obstaclePoints = env.getAllObstaclePoints();
+        for (unsigned i = 0; i < obstaclePoints.size() && admissible; i++) {
+            Arc movedArc(arc);
+            admissible &= !robotBody.isIntersect(movedArc.translateToObstacleCornerArc(obstaclePoints[i]));
+        }
     }
     return admissible;
 }