int main() {
double p1, p2, q1, q2, r1, r2;
double pi = 3.141592653589793;
while(scanf("%lf %lf %lf %lf %lf %lf\n", &p1, &p2, &q1, &q2, &r1, &r2) != EOF) {
// Perpendicular bisector of a chord must pass through the centre.
// Sometimes, picking the wrong ones causes infinity things to happen, so we have to try all of them.
double m1 = -1/((p2-q2)/(p1-q1)); // Perpendicular
double midpoint1x = (p1+q1)/2;
double midpoint1y = (p2+q2)/2;
double m2 = -1/((q2-r2)/(q1-r1));
double midpoint2x = (r1+q1)/2;
double midpoint2y = (r2+q2)/2;
double m3 = -1/((r2-p2)/(r1-p1));
double midpoint3x = (r1+p1)/2;
double midpoint3y = (r2+p2)/2;
// y - y1 = m(x-x1)
double radius = calculateRadius(p1, p2, m1, midpoint1x, midpoint1y, m2, midpoint2x, midpoint2y);
if (isnan(radius) != 0) {
radius = calculateRadius(p1, p2, m1, midpoint1x, midpoint1y, m3, midpoint3x, midpoint3y);
}
if (isnan(radius) != 0) {
radius = calculateRadius(p1, p2, m2, midpoint2x, midpoint2y, m3, midpoint3x, midpoint3y);
}
printf("%0.2f\n", 2*pi*radius);
}
return EXIT_SUCCESS;
}
void BoxMesh::construct()
{
int i ;
Box bounds = getBounds() ;
Point center = (bounds.max - bounds.min) / 2 ;
// Vertex
verts.push_back(Point(bounds.min.x(), bounds.min.y(), bounds.min.z()));
verts.push_back(Point(bounds.max.x(), bounds.min.y(), bounds.min.z()));
verts.push_back(Point(bounds.min.x(), bounds.max.y(), bounds.min.z()));
verts.push_back(Point(bounds.max.x(), bounds.max.y(), bounds.min.z()));
verts.push_back(Point(bounds.min.x(), bounds.min.y(), bounds.max.z()));
verts.push_back(Point(bounds.max.x(), bounds.min.y(), bounds.max.z()));
verts.push_back(Point(bounds.min.x(), bounds.max.y(), bounds.max.z()));
verts.push_back(Point(bounds.max.x(), bounds.max.y(), bounds.max.z()));
// Texture coordinates
for (i = 0 ; i < 8 ; i++)
tverts.push_back(Point2D(0,0));
// Indices
int inds[] = { 0, 4, 1, 5, 3, 7, 2, 6, 0, 4, 6, 7, 4, 5, 0, 1, 2, 3 } ;
for (i = 0 ; i < sizeof(inds)/sizeof(inds[0]) ; i++)
indices.push_back(inds[i]);
// Primitives
Primitive p ;
p.firstElement = 0 ;
p.numElements = 10 ;
p.type = Primitive::NoMaterial | Primitive::Strip | Primitive::Indexed ;
primitives.push_back(p) ;
p.firstElement = 10 ;
p.numElements = 4 ;
primitives.push_back(p) ;
p.firstElement = 14 ;
primitives.push_back(p) ;
// Normals
std::vector<Point>::iterator ptr ;
for (ptr = verts.begin() ; ptr != verts.end() ; ptr++)
{
Point normal ;
normal = *ptr - center ;
normal.normalize() ;
normals.push_back (normal) ;
enormals.push_back (encodeNormal(normal)) ;
}
// Other stuff
setFrames (1) ;
setParent (-1) ;
calculateCenter() ;
calculateRadius() ;
}
Mesh(const VertsRangeT& verts, int coordsPerVertex, GLenum renderStyle, const ColorsRangeT& colors, int componentsPerColor)
: coordsPerVertex_(coordsPerVertex)
, componentsPerColor_(componentsPerColor)
, renderStyle_(renderStyle)
, radius_(0)
{
boost::range::push_back(vertexes_, verts);
boost::range::push_back(colors_, colors);
calculateCentroid();
calculateRadius();
}
ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
{
if (lastGoalMotion_) {
OMPL_INFORM("solve() called before clear(); returning previous solution");
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
}
else if (Open_.size() > 0)
{
OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
clear();
}
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
Motion *initMotion = nullptr;
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
// Add start states to V (nn_) and Open
while (const base::State *st = pis_.nextStart())
{
initMotion = new Motion(si_);
si_->copyState(initMotion->getState(), st);
Open_.insert(initMotion);
initMotion->setSetType(Motion::SET_OPEN);
initMotion->setCost(opt_->initialCost(initMotion->getState()));
nn_->add(initMotion); // V <-- {x_init}
}
if (!initMotion)
{
OMPL_ERROR("Start state undefined");
return base::PlannerStatus::INVALID_START;
}
// Sample N free states in the configuration space
if (!sampler_)
sampler_ = si_->allocStateSampler();
sampleFree(ptc);
assureGoalIsSampled(goal);
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
// Calculate the nearest neighbor search radius
/// \todo Create a PRM-like connection strategy
if (nearestK_)
{
NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
(boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
log((double)nn_->size()));
OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
}
else
{
NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
OMPL_DEBUG("Using radius of %f", NNr_);
}
// Execute the planner, and return early if the planner returns a failure
bool plannerSuccess = false;
bool successfulExpansion = false;
Motion *z = initMotion; // z <-- xinit
saveNeighborhood(z);
while (!ptc)
{
if ((plannerSuccess = goal->isSatisfied(z->getState())))
break;
successfulExpansion = expandTreeFromNode(&z);
if (!extendedFMT_ && !successfulExpansion)
break;
else if (extendedFMT_ && !successfulExpansion)
{
//Apply RRT*-like connections: sample and connect samples to tree
std::vector<Motion*> nbh;
std::vector<base::Cost> costs;
std::vector<base::Cost> incCosts;
std::vector<std::size_t> sortedCostIndices;
// our functor for sorting nearest neighbors
CostIndexCompare compareFn(costs, *opt_);
Motion *m = new Motion(si_);
while (!ptc && Open_.empty())
{
sampler_->sampleUniform(m->getState());
if (!si_->isValid(m->getState()))
continue;
if (nearestK_)
nn_->nearestK(m, NNk_, nbh);
else
nn_->nearestR(m, NNr_, nbh);
// Get neighbours in the tree.
std::vector<Motion*> yNear;
yNear.reserve(nbh.size());
for (std::size_t j = 0; j < nbh.size(); ++j)
{
if (nbh[j]->getSetType() == Motion::SET_CLOSED)
{
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
yNear.push_back(nbh[j]);
}
else
yNear.push_back(nbh[j]);
}
}
// Sample again if the new sample does not connect to the tree.
if (yNear.empty())
continue;
// cache for distance computations
//
// Our cost caches only increase in size, so they're only
// resized if they can't fit the current neighborhood
if (costs.size() < yNear.size())
{
costs.resize(yNear.size());
incCosts.resize(yNear.size());
sortedCostIndices.resize(yNear.size());
}
// Finding the nearest neighbor to connect to
// By default, neighborhood states are sorted by cost, and collision checking
// is performed in increasing order of cost
//
// calculate all costs and distances
for (std::size_t i = 0 ; i < yNear.size(); ++i)
{
incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
}
// sort the nodes
//
// we're using index-value pairs so that we can get at
// original, unsorted indices
for (std::size_t i = 0; i < yNear.size(); ++i)
sortedCostIndices[i] = i;
std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
compareFn);
// collision check until a valid motion is found
for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
i != sortedCostIndices.begin() + yNear.size();
++i)
{
if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
{
m->setParent(yNear[*i]);
yNear[*i]->getChildren().push_back(m);
const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
m->setSetType(Motion::SET_OPEN);
nn_->add(m);
saveNeighborhood(m);
updateNeighborhood(m,nbh);
Open_.insert(m);
z = m;
break;
}
}
} // while (!ptc && Open_.empty())
} // else if (extendedFMT_ && !successfulExpansion)
} // While not at goal
if (plannerSuccess)
{
// Return the path to z, since by definition of planner success, z is in the goal region
lastGoalMotion_ = z;
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
} // if plannerSuccess
else
{
// Planner terminated without accomplishing goal
return base::PlannerStatus(false, false);
}
}
void CylinderMesh::construct()
{
int i, firstTop ;
int steps = (unsigned int)(complexity * 64.0f) ;
float angle ;
Point normal ;
Box bounds = getBounds() ;
Point center = getCenter() ;
float radius = getRadius() ;
Primitive p ;
p.type = Primitive::NoMaterial | Primitive::Strip | Primitive::Indexed ;
if (steps < 4) steps = 4 ;
if (steps > 64) steps = 64 ;
steps &= ~1 ;
// Bottom
addVertex (center.x(), center.y(), bounds.min.z()) ;
for (angle = 0.0f, i = 0 ; i < steps ; i++, angle += 2.0f*M_PI/(float)steps)
{
addVertex (cosf(angle)*radius + center.x(), sinf(angle)*radius + center.y(),
bounds.min.z()) ;
}
for (i = 1 ; i <= steps ; i++)
{
p.firstElement = indices.size() ;
p.numElements = 3 ;
indices.push_back (0) ;
indices.push_back (i) ;
indices.push_back (i == steps ? 1 : i+1) ;
primitives.push_back(p) ;
}
// Top
firstTop = verts.size() ;
addVertex (center.x(), center.y(), bounds.max.z()) ;
for (angle = 0.0f, i = 0 ; i < steps ; i++, angle += 2.0f*M_PI/(float)steps)
{
addVertex (cosf(angle)*radius + center.x(), sinf(angle)*radius + center.y(),
bounds.max.z()) ;
}
for (i = 1 ; i <= steps ; i++)
{
p.firstElement = indices.size() ;
p.numElements = 3 ;
indices.push_back (firstTop) ;
indices.push_back (firstTop+(i == steps ? 1 : i+1)) ;
indices.push_back (firstTop+i) ;
primitives.push_back(p) ;
}
// Walls
int pos ;
for (pos = indices.size(), i = 0 ; i < steps-1 ; i++, pos += 4)
{
indices.push_back (i+1) ;
indices.push_back (firstTop+i+1) ;
indices.push_back (i+2) ;
indices.push_back (firstTop+i+2) ;
p.firstElement = pos ;
p.numElements = 4 ;
primitives.push_back(p) ;
}
indices.push_back (i+1) ;
indices.push_back (firstTop+i+1) ;
indices.push_back (1) ;
indices.push_back (firstTop+1) ;
p.firstElement = pos ;
p.numElements = 4 ;
primitives.push_back(p) ;
// Other stuff
setFrames (1) ;
setParent (-1) ;
calculateBounds() ;
calculateCenter() ;
calculateRadius() ;
}
void RotateOnce::run(void* msg)
{
/*PROTECTED REGION ID(run1467397900274) ENABLED START*/ //Add additional options here
cout.precision(4);
if (this->isSuccess())
{
return;
}
msl_actuator_msgs::MotionControl mc;
rotationSpeed = getLimitedRotationSpeed(rotationSpeed + ACCELERATION); // accelerate in each iteration until max rotation speed is reached
mc.motion.rotation = rotationSpeed;
send(mc);
double currentIMUBearing = getIMUBearing();
double currentMotionBearing = getMotionBearing();
double circDiff = circularDiff(currentIMUBearing, lastIMUBearing);
double iR, mR;
bool isFullIMURotation = false;
bool isFullMotionRotation = false;
iR = updateRotationCount(currentIMUBearing, lastIMUBearing, imuRotations, isFullIMURotation);
mR = updateRotationCount(currentMotionBearing, lastMotionBearing, motionRotations, isFullMotionRotation);
if (isFullIMURotation)
{
// if this is the first full IMU rotation, initialize the IMU rotation offset
if (!diffOffsetInitialized)
{
diffOffset = iR - mR;
diffOffsetInitialized = true;
}
double currentDiff = (iR - diffOffset) - mR;
#ifdef ROT_CALIB_DEBUG_ONLY
cout << currentIMUBearing << "\t";
cout << "\t";
cout << currentMotionBearing << "\t";
cout << "\t";
cout << iR << "\t" << mR << "\t" << currentDiff << endl;
#endif
#ifndef ROT_CALIB_DEBUG_ONLY
int percent = floor(100 * currentDiff / MIN_BEARING_DIFF_FOR_REGRESSION);
cout << floor(iR) << "/" << MAX_ROTATIONS << " rotations, difference sums up to " << percent
<< "% of the calibration threshold" << endl;
#endif
logIMUMotionDifference(currentDiff);
if (iR > MAX_ROTATIONS)
{
// MAX_ROTATIONS reached - calibration finished!
cout << "MAX_ROTATIONS reached - calibration finished!" << endl;
this->setSuccess(true);
}
else if (iR > MIN_ROTATIONS && fabs(currentDiff) > MIN_BEARING_DIFF_FOR_REGRESSION)
{
// MIN_BEARING_DIFF_FOR_REGRESSION reached - we can start a regression calculation in order to improve on the RobotRadius
cout
<< "MIN_BEARING_DIFF_FOR_REGRESSION reached - we can start a regression calculation in order to improve on the RobotRadius"
<< endl;
calculateRadius();
this->setFailure(true);
}
}
/*PROTECTED REGION END*/
}
