float Planning::Path::length(const Geometry2d::Point &pt) const
{
float dist = -1;
float length = 0;
if (points.empty())
{
return 0;
}
for (unsigned int i = 0; i < (points.size() - 1); ++i)
{
Geometry2d::Segment s(points[i], points[i+1]);
//add the segment length
length += s.length();
const float d = s.distTo(pt);
//if point closer to this segment
if (dist < 0 || d < dist)
{
//closest point on segment
Geometry2d::Point p = s.nearestPoint(pt);
//new best distance
dist = d;
//reset running length count
length = 0;
length += p.distTo(s.pt[1]);
}
}
return length;
}
// Returns the index of the point in this path nearest to pt.
int Planning::Path::nearestIndex(const Geometry2d::Point &pt) const
{
if (points.size() == 0)
{
return -1;
}
int index = 0;
float dist = pt.distTo(points[0]);
for (unsigned int i=1 ; i<points.size(); ++i)
{
float d = pt.distTo(points[i]);
if (d < dist)
{
dist = d;
index = i;
}
}
return index;
}
Geometry2d::ShapeSet createRobotObstacles(const std::vector<ROBOT*>& robots,
const RobotMask& mask,
Geometry2d::Point currentPosition,
float checkRadius) const {
Geometry2d::ShapeSet result;
for (size_t i = 0; i < mask.size(); ++i)
if (mask[i] > 0 && robots[i] && robots[i]->visible) {
if (currentPosition.distTo(robots[i]->pos) <= checkRadius) {
result.add(std::shared_ptr<Geometry2d::Shape>(
new Geometry2d::Circle(robots[i]->pos, mask[i])));
}
}
return result;
}
void SimFieldView::mousePressEvent(QMouseEvent* me) {
Geometry2d::Point pos = _worldToTeam * _screenToWorld * me->pos();
std::shared_ptr<LogFrame> frame = currentFrame();
if (me->button() == Qt::LeftButton && frame) {
_dragRobot = -1;
for (const LogFrame::Robot& r : frame->self()) {
if (pos.nearPoint(r.pos(), Robot_Radius)) {
_dragRobot = r.shell();
_dragRobotBlue = frame->blue_team();
break;
}
}
for (const LogFrame::Robot& r : frame->opp()) {
if (pos.nearPoint(r.pos(), Robot_Radius)) {
_dragRobot = r.shell();
_dragRobotBlue = !frame->blue_team();
break;
}
}
if (_dragRobot < 0) {
placeBall(me->pos());
}
_dragMode = DRAG_PLACE;
} else if (me->button() == Qt::RightButton && frame) {
if (frame->has_ball() &&
pos.nearPoint(frame->ball().pos(), Ball_Radius)) {
// Drag to shoot the ball
_dragMode = DRAG_SHOOT;
_dragTo = pos;
} else {
// Look for a robot selection
int newID = -1;
for (int i = 0; i < frame->self_size(); ++i) {
if (pos.distTo(frame->self(i).pos()) < Robot_Radius) {
newID = frame->self(i).shell();
break;
}
}
if (newID != frame->manual_id()) {
robotSelected(newID);
}
}
}
}
bool Circle::tangentPoints(const Geometry2d::Point &src,
Geometry2d::Point* p1, Geometry2d::Point* p2) const
{
if (!p1 && !p2)
{
return false;
}
const float dist = src.distTo(center);
if (dist < _r)
{
return false;
}
else if (dist == _r)
{
if (p1)
{
*p1 = src;
}
if (p2)
{
*p2 = src;
}
}
else
{
//source is outside of circle
const float theta = asin(_r/dist);
const float degT = theta * 180.0f / M_PI;
if (p1)
{
Point final = center;
final.rotate(src, degT);
*p1 = final;
}
if (p2)
{
Point final = center;
final.rotate(src, -degT);
*p2 = final;
}
/**
* Generates a Cubic Bezier Path based on Albert's random Bezier Velocity Path
* Algorithm
*/
std::vector<InterpolatedPath::Entry> RRTPlanner::generateVelocityPath(
const std::vector<CubicBezierControlPoints>& controlPoints,
const MotionConstraints& motionConstraints, Geometry2d::Point vi,
Geometry2d::Point vf, int interpolations) {
// Interpolate Through Bezier Path
vector<Point> newPoints, newPoints1stDerivative, newPoints2ndDerivative;
vector<float> newPointsCurvature, newPointsDistance, newPointsSpeed;
float totalDistance = 0;
const float maxAceleration = motionConstraints.maxAcceleration;
for (const CubicBezierControlPoints& controlPoint : controlPoints) {
Point p0 = controlPoint.p0;
Point p1 = controlPoint.p1;
Point p2 = controlPoint.p2;
Point p3 = controlPoint.p3;
for (int j = 0; j < interpolations; j++) {
float t = (((float)j / (float)(interpolations)));
Geometry2d::Point pos =
pow(1.0 - t, 3) * p0 + 3.0 * pow(1.0 - t, 2) * t * p1 +
3 * (1.0 - t) * pow(t, 2) * p2 + pow(t, 3) * p3;
// Derivitive 1
// 3 k (-(A (-1 + k t)^2) + k t (2 C - 3 C k t + D k t) + B (1 - 4 k
// t + 3 k^2 t^2))
Geometry2d::Point d1 = 3 * pow(1 - t, 2) * (p1 - p0) +
6 * (1 - t) * t * (p2 - p1) +
3 * pow(t, 2) * (p3 - p2);
// Derivitive 2
// https://en.wikipedia.org/wiki/B%C3%A9zier_curve#Cubic_B.C3.A9zier_curves
// B''(t) = 6(1-t)(P2 - 2*P1 + P0) + 6*t(P3 - 2 * P2 + P1)
Geometry2d::Point d2 =
6 * (1 - t) * (p2 - 2 * p1 + p0) + 6 * t * (p3 - 2 * p2 + p1);
// https://en.wikipedia.org/wiki/Curvature#Local_expressions
// K = |x'*y'' - y'*x''| / (x'^2 + y'^2)^(3/2)
float curvature =
std::abs(d1.x * d2.y - d1.y * d2.x) /
std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);
// Handle 0 velocity case
if (isnan(curvature)) {
curvature = 0;
}
assert(curvature >= 0);
if (!newPoints.empty()) {
float distance = pos.distTo(newPoints.back());
totalDistance += distance;
}
newPointsDistance.push_back(totalDistance);
newPoints.push_back(pos);
newPoints1stDerivative.push_back(d1);
newPoints2ndDerivative.push_back(d2);
newPointsCurvature.push_back(curvature);
// Isolated maxSpeed based on Curvature
// Curvature = 1/Radius of Curvature
// vmax = sqrt(acceleartion/abs(Curvature))
float constantMaxSpeed = std::sqrt(maxAceleration / curvature);
newPointsSpeed.push_back(constantMaxSpeed);
}
}
// Get last point in Path
CubicBezierControlPoints lastControlPoint = controlPoints.back();
Point p0 = lastControlPoint.p0;
Point p1 = lastControlPoint.p1;
Point p2 = lastControlPoint.p2;
Point p3 = lastControlPoint.p3;
Point pos = p3;
Point d1 = vf;
Geometry2d::Point d2 = 6 * (1) * (p3 - 2 * p2 + p1);
float curvature = std::abs(d1.x * d2.y - d1.y * d2.x) /
std::pow(std::pow(d1.x, 2) + std::pow(d1.y, 2), 1.5);
// handle 0 velcoity case
if (isnan(curvature)) {
curvature = 0;
}
totalDistance += pos.distTo(newPoints.back());
newPoints.push_back(pos);
newPoints1stDerivative.push_back(vf);
newPoints2ndDerivative.push_back(d2);
newPointsCurvature.push_back(curvature);
newPointsDistance.push_back(totalDistance);
newPointsSpeed[0] = vi.mag();
newPointsSpeed.push_back(vf.mag());
// Velocity Profile Generation
// Forward Smoothing
const float size = newPoints.size();
assert(size == newPoints.size());
assert(size == newPoints1stDerivative.size());
assert(size == newPoints2ndDerivative.size());
assert(size == newPointsDistance.size());
assert(size == newPointsSpeed.size());
assert(size == newPointsCurvature.size());
// Generate Velocity Profile from Interpolation of Bezier Path
// Forward Constraints
for (int i = 1; i < size; i++) {
int i1 = i - 1;
int i2 = i;
newPointsSpeed[i2] = oneStepLimitAcceleration(
maxAceleration, newPointsDistance[i1], newPointsSpeed[i1],
newPointsCurvature[i1], newPointsDistance[i2], newPointsSpeed[i2],
newPointsCurvature[i2]);
}
// Backwards Constraints
for (int i = size - 2; i >= 0; i--) {
int i1 = i + 1;
int i2 = i;
newPointsSpeed[i2] = oneStepLimitAcceleration(
maxAceleration, newPointsDistance[i1], newPointsSpeed[i1],
newPointsCurvature[i1], newPointsDistance[i2], newPointsSpeed[i2],
newPointsCurvature[i2]);
}
float totalTime = 0;
vector<InterpolatedPath::Entry> entries;
for (int i = 0; i < size; i++) {
float currentSpeed = newPointsSpeed[i];
float distance = newPointsDistance[i];
if (i != 0) {
distance -= newPointsDistance[i - 1];
float averageSpeed = (currentSpeed + newPointsSpeed[i - 1]) / 2.0;
float deltaT = distance / averageSpeed;
totalTime += deltaT;
}
Point point = newPoints[i];
Point vel = newPoints1stDerivative[i].normalized();
entries.emplace_back(MotionInstant(point, vel * currentSpeed),
totalTime);
}
return entries;
}
double distance(const Geometry2d::Point& from,
const Geometry2d::Point& to) const {
return from.distTo(to);
}