Point2d MgArc::_getHandlePoint(int index) const
{
if (index == 1) {
return getStartPoint();
}
if (index == 2) {
return getEndPoint();
}
if (index == 3) {
return getMidPoint();
}
if (index == 4) {
return (getStartPoint() * 2 + getCenter()) / 3;
}
if (index == 5) {
return (getEndPoint() * 2 + getCenter()) / 3;
}
if (index == 6) {
return getStartPoint() + getStartTangent();
}
if (index == 7) {
return getEndPoint() + getEndTangent();
}
return getCenter();
}
void REllipse::moveStartPoint(const RVector& pos, bool changeAngleOnly) {
if (changeAngleOnly) {
startParam = getParamTo(pos);
}
else {
RVector ep = getEndPoint();
double distOri = ep.getDistanceTo(getStartPoint());
double angleOri = ep.getAngleTo(getStartPoint());
if (distOri<RS::PointTolerance) {
return;
}
double distNew = ep.getDistanceTo(pos);
double angleNew = ep.getAngleTo(pos);
double factor = distNew / distOri;
if (factor<RS::PointTolerance) {
return;
}
double angleDelta = angleNew - angleOri;
center.scale(factor, ep);
center.rotate(angleDelta, ep);
majorPoint.scale(factor);
majorPoint.rotate(angleDelta);
}
}
QPainterPath CurveGraphicsItem::obtainCurvePath() const {
if (useMidPoint_) {
return obtainCurvePath(getStartPoint(), getMidPoint()) +
obtainCurvePath(getMidPoint(), getEndPoint());
} else {
return obtainCurvePath(getStartPoint(), getEndPoint());
}
}
C3dBoundedLine::C3dBoundedLine(const C3dWorldPoint & p1, const C3dWorldPoint & p2) : T3dBoundedLine_base(p1, p2)
{
if(IS_DEBUG)
{
p1.checkInit();
p2.checkInit();
CHECK(getStartPoint() == getFinishPoint() && getStartPoint() != C3dWorldPoint::uninit(), "Line end points are equal");
}
}
QPainterPath ArrowLinkItem::originalShape() const
{
QPointF c1 = getControlItemPosFirst();
QPointF c2 = getControlItemPosSecond();
QPainterPath path(getStartPoint());
path.cubicTo(c1, c2, getEndPoint());
path.cubicTo(c2, c1, getStartPoint());
return path;
}
float MgArc::getSweepAngle() const
{
if (!mgIsZero(_sweepAngle)) {
return _sweepAngle;
}
const float midAngle = (getMidPoint() - getCenter()).angle2();
const float startAngle = getStartAngle();
const float endAngle = getEndAngle();
if (mgEquals(midAngle, startAngle) && mgEquals(startAngle, endAngle)) {
return endAngle - startAngle;
}
Tol tol(getRadius() * 1e-3f, 1e-4f);
if (getStartPoint().isEqualTo(getEndPoint(), tol)
&& (getMidPoint() + (getStartPoint() + getEndPoint()) / 2).isEqualTo(2 * getCenter(), tol)) {
return _M_2PI;
}
float startAngle2 = startAngle;
float midAngle2 = midAngle;
float endAngle2 = endAngle;
// 先尝试看是否为逆时针方向：endAngle2 > midAngle2 > startAngle2 >= 0
if (startAngle2 < 0)
startAngle2 += _M_2PI;
while (midAngle2 < startAngle2)
midAngle2 += _M_2PI;
while (endAngle2 < midAngle2)
endAngle2 += _M_2PI;
if (fabsf(startAngle2 + endAngle2 - 2 * midAngle2) < _M_PI_6
&& endAngle2 - startAngle2 < _M_2PI) {
return endAngle2 - startAngle2;
}
// 再尝试看是否为顺时针方向：endAngle2 < midAngle2 < startAngle2 <= 0
startAngle2 = startAngle;
midAngle2 = midAngle;
endAngle2 = endAngle;
if (startAngle2 > 0)
startAngle2 -= _M_2PI;
while (midAngle2 > startAngle2)
midAngle2 -= _M_2PI;
while (endAngle2 > midAngle2)
endAngle2 -= _M_2PI;
if (fabsf(startAngle2 + endAngle2 - 2 * midAngle2) < _M_PI_6) {
if (endAngle2 - startAngle2 > -_M_2PI)
return endAngle2 - startAngle2;
return mgbase::toRange(endAngle2 - startAngle2, -_M_2PI, 0);
}
return endAngle - startAngle; // error
}
void LaneLineTile::writeLine(std::ostream &out, double from, double to, double offset, double width, std::string color) const
{
if (from >= to)
return;
Vector2 orthogonal = getStartDirection().getPerpendicularVectorRight();
Vector2 p = getStartPoint() + getStartDirection() * from + orthogonal * offset;
Vector2 q = getStartPoint() + getStartDirection() * to + orthogonal * offset;
out << "<line x1=\"" << p[0] << "\" y1=\"" << p[1] << "\" x2=\"" << q[0] << "\" y2=\"" << q[1] << "\" style=\"stroke:" << color << "; stroke-width:" << width << ";\" />\n";
}
bool RectangularRegion::hitRightModifyPoint(const QPoint &point) {
bool result = false;
QPoint rightModifyPoint(getStartPoint().x() + getWidth(), getStartPoint().y() + getHeight() / 2);
if (Region::distance(point, rightModifyPoint) <= SQUARE(MODIFY_POINT_RADIUS)){
result = true;
setIsRightSelected();
} else {
clearIsRightSelected();
}
return result;
}
bool MgArc::_draw(int mode, GiGraphics& gs, const GiContext& ctx, int segment) const
{
bool ret = gs.drawArc(&ctx, getCenter(), getRadius(), 0, getStartAngle(), getSweepAngle());
if (mode > 0) {
GiContext ctxln(0, GiColor(0, 126, 0, 64), kGiLineDashDot);
gs.drawLine(&ctxln, getCenter(), getStartPoint());
gs.drawLine(&ctxln, getCenter(), getEndPoint());
gs.drawLine(&ctxln, getStartPoint(), getStartPoint() + getStartTangent());
gs.drawLine(&ctxln, getEndPoint(), getEndPoint() + getEndTangent());
}
return __super::_draw(mode, gs, ctx, segment) || ret;
}
RBox RArc::getBoundingBox() const {
RVector minV;
RVector maxV;
double minX = qMin(getStartPoint().x, getEndPoint().x);
double minY = qMin(getStartPoint().y, getEndPoint().y);
double maxX = qMax(getStartPoint().x, getEndPoint().x);
double maxY = qMax(getStartPoint().y, getEndPoint().y);
if (getStartPoint().getDistanceTo(getEndPoint()) < 1.0e-6 && getRadius()
> 1.0e5) {
minV = RVector(minX, minY);
maxV = RVector(maxX, maxY);
return RBox(minV, maxV);
}
double a1 = RMath::getNormalizedAngle(!isReversed() ? startAngle : endAngle);
double a2 = RMath::getNormalizedAngle(!isReversed() ? endAngle : startAngle);
// check for left limit:
if ((a1<M_PI && a2>M_PI) ||
(a1>a2-1.0e-12 && a2>M_PI) ||
(a1>a2-1.0e-12 && a1<M_PI) ) {
minX = qMin(center.x - radius, minX);
}
// check for right limit:
if (a1 > a2-1.0e-12) {
maxX = qMax(center.x + radius, maxX);
}
// check for bottom limit:
if ((a1<(M_PI_2*3) && a2>(M_PI_2*3)) ||
(a1>a2-1.0e-12 && a2>(M_PI_2*3)) ||
(a1>a2-1.0e-12 && a1<(M_PI_2*3)) ) {
minY = qMin(center.y - radius, minY);
}
// check for top limit:
if ((a1<M_PI_2 && a2>M_PI_2) ||
(a1>a2-1.0e-12 && a2>M_PI_2) ||
(a1>a2-1.0e-12 && a1<M_PI_2) ) {
maxY = qMax(center.y + radius, maxY);
}
minV = RVector(minX, minY);
maxV = RVector(maxX, maxY);
return RBox(minV, maxV);
}
bool REllipse::switchMajorMinor() {
if (fabs(ratio) < RS::PointTolerance) {
return false;
}
RVector vp_start=getStartPoint();
RVector vp_end=getStartPoint();
RVector vp=getMajorPoint();
setMajorPoint(RVector(-ratio*vp.y, ratio*vp.x));
setRatio(1.0/ratio);
setStartParam(getParamTo(vp_start));
setEndParam(getParamTo(vp_end));
return true;
}
bool C3dBoundedLine::planeIntersect(const C3dWorldPoint & n, const double d, C3dWorldPoint & intersection) const
{
double t = (d-n.dot(getStartPoint()))/n.dot(getFinishPoint()-getStartPoint());
if(t>1 || t<0)
return false;
intersection = (getStartPoint() + t*(getFinishPoint()-getStartPoint())).eval();
//cout << intersection.transpose() << " intersects plane at t=" << t << endl;
if(IS_DEBUG) CHECK(!zero(intersection.dot(n) - d), "plane intersect computation failed");
return true;
}
C2dBoundedLine::eLineSide C2dBoundedLine::side(const C2dImagePointPx & point) const
{
C2dImagePointPx bottom, top;
if(getStartPoint().y() > getFinishPoint().y()) {
bottom=getStartPoint();
top=getFinishPoint();
} else {
bottom=getFinishPoint();
top=getStartPoint();
}
C2dImagePointPx pll = bottom-top;
C2dImagePointPx perp(-pll.y(), pll.x());
return (perp.dot(point-bottom) < 0) ? eLeftOfLine : eRightOfLine;
}
float MgArc::_hitTest(const Point2d& pt, float tol, MgHitResult& res) const
{
Point2d points[16];
int n = mgcurv::arcToBezier(points, getCenter(), getRadius(), 0, getStartAngle(), getSweepAngle());
float dist, distMin = _FLT_MAX;
Point2d ptTemp;
if (_subtype > 0) {
dist = mglnrel::ptToLine(getCenter(), getStartPoint(), pt, ptTemp);
if (dist <= tol && dist < distMin) {
distMin = dist;
res.nearpt = ptTemp;
}
dist = mglnrel::ptToLine(getCenter(), getEndPoint(), pt, ptTemp);
if (dist <= tol && dist < distMin) {
distMin = dist;
res.nearpt = ptTemp;
}
}
for (int i = 0; i + 3 < n; i += 3) {
mgnear::nearestOnBezier(pt, points + i, ptTemp);
dist = pt.distanceTo(ptTemp);
if (dist <= tol && dist < distMin) {
distMin = dist;
res.nearpt = ptTemp;
}
}
return distMin;
}
void RSpline::print(QDebug dbg) const {
dbg.nospace() << "RSpline(";
RShape::print(dbg);
dbg.nospace() << ", degree: " << getDegree();
dbg.nospace() << ", order: " << getOrder();
dbg.nospace() << ", closed: " << isClosed();
dbg.nospace() << ", periodic: " << isPeriodic();
dbg.nospace() << ", start point: " << getStartPoint();
dbg.nospace() << ", end point: " << getEndPoint();
QList<RVector> controlPoints = getControlPointsWrapped();
dbg.nospace() << ",\ncontrolPoints (" << controlPoints.count() << "): ";
for (int i=0; i<controlPoints.count(); ++i) {
dbg.nospace() << i << ": " << controlPoints.at(i);
}
QList<RVector> fitPoints = getFitPoints();
dbg.nospace() << ",\nfitPoints (" << fitPoints.count() << "): ";
for (int i=0; i<fitPoints.count(); ++i) {
dbg.nospace() << i << ": " << fitPoints.at(i) << ", ";
}
QList<double> knotVector = getKnotVector();
dbg.nospace() << ",\nknots (" << knotVector.count() << "): ";
for (int i=0; i<knotVector.count(); ++i) {
dbg.nospace() << i << ": " << knotVector.at(i) << ", ";
}
knotVector = getActualKnotVector();
dbg.nospace() << ",\ninternally used knots (" << knotVector.count() << "): ";
for (int i=0; i<knotVector.count(); ++i) {
dbg.nospace() << i << ": " << knotVector.at(i) << ", ";
}
}
void setSignature(int col, int index, long value, int one, int two, int three, int four)
{
//set the at column - col, and the corresponding row infomation of the blocks four element
long start = getStartPoint(col);
if (index == -1) //means add a new value
{
signatures[start + filled_signature[col]] = value;
signatures_one[start + filled_signature[col]] = one;
signatures_two[start + filled_signature[col]] = two;
signatures_three[start + filled_signature[col]] = three;
signatures_four[start + filled_signature[col]] = four;
correspond_col[start + filled_signature[col]] = col;
filled_signature[col] = filled_signature[col] + 1;
}
else
{ //change an exist value
signatures[start + index] = value;
signatures_one[start + index] = one;
signatures_two[start + index] = two;
signatures_three[start + index] = three;
signatures_four[start + index] = four;
correspond_col[start + index] = col;
}
// printf("For col %d get start point %ld,setting to %ld,filled signatures is %ld\n", col, start, value, filled_signature[col]);
}
bool RArcData::moveReferencePoint(const RVector& referencePoint, const RVector& targetPoint) {
bool ret = false;
if (referencePoint.equalsFuzzy(center)) {
center = targetPoint;
ret = true;
} else if (referencePoint.equalsFuzzy(getStartPoint())) {
moveStartPoint(targetPoint);
ret = true;
} else if (referencePoint.equalsFuzzy(getEndPoint())) {
moveEndPoint(targetPoint);
ret = true;
}
else if (referencePoint.equalsFuzzy(center + RVector(radius, 0)) ||
referencePoint.equalsFuzzy(center + RVector(0, radius)) ||
referencePoint.equalsFuzzy(center - RVector(radius, 0)) ||
referencePoint.equalsFuzzy(center - RVector(0, radius))) {
radius = center.getDistanceTo(targetPoint);
ret = true;
}
else if (referencePoint.equalsFuzzy(getMiddlePoint())) {
moveMiddlePoint(targetPoint);
ret = true;
}
return ret;
}
bool RSpline::mirror(const RLine& axis) {
RVector sp = getStartPoint();
RVector ep = getEndPoint();
for (int i=0; i<controlPoints.size(); i++) {
controlPoints[i].mirror(axis);
}
for (int i=0; i<fitPoints.size(); i++) {
fitPoints[i].mirror(axis);
}
RVector absTan = sp+tangentStart;
absTan.mirror(axis);
sp.mirror(axis);
tangentStart = absTan-sp;
absTan = ep+tangentEnd;
absTan.mirror(axis);
ep.mirror(axis);
tangentEnd = absTan-ep;
update();
return true;
}
RBox REllipse::getBoundingBox() const {
double radius1 = getMajorRadius();
double radius2 = getMinorRadius();
double angle = getAngle();
double a1 = ((!isReversed()) ? startParam : endParam);
double a2 = ((!isReversed()) ? endParam : startParam);
RVector startPoint = getStartPoint();
RVector endPoint = getEndPoint();
double minX = qMin(startPoint.x, endPoint.x);
double minY = qMin(startPoint.y, endPoint.y);
double maxX = qMax(startPoint.x, endPoint.x);
double maxY = qMax(startPoint.y, endPoint.y);
// kind of a brute force. TODO: exact calculation
RVector vp;
double a = a1;
do {
vp.set(center.x + radius1 * cos(a),
center.y + radius2 * sin(a));
vp.rotate(angle, center);
minX = qMin(minX, vp.x);
minY = qMin(minY, vp.y);
maxX = qMax(maxX, vp.x);
maxY = qMax(maxY, vp.y);
a += 0.03;
} while (RMath::isAngleBetween(a, a1, a2, false) && a<4*M_PI);
return RBox(RVector(minX,minY), RVector(maxX,maxY));
}
QList<RVector> RSpline::getEndPoints() const {
QList<RVector> ret;
ret.append(getStartPoint());
ret.append(getEndPoint());
return ret;
}
int isStartPointValid(const Track *t)
{
if (NULL == t)
return 0;
const GeoPoint p = getStartPoint(t);
return isValidPoint(&p);
}
bool CapsuleShape::findRayIntersection(const glm::vec3& rayStart, const glm::vec3& rayDirection, float& distance) const {
glm::vec3 capsuleStart, capsuleEnd;
getStartPoint(capsuleStart);
getEndPoint(capsuleEnd);
// NOTE: findRayCapsuleIntersection returns 'true' with distance = 0 when rayStart is inside capsule.
// TODO: implement the raycast to return inside surface intersection for the internal rayStart.
return findRayCapsuleIntersection(rayStart, rayDirection, capsuleStart, capsuleEnd, _radius, distance);
}
bool RArc::stretch(const RPolyline& area, const RVector& offset) {
bool ret = false;
if (area.contains(getStartPoint()) && area.contains(getEndPoint())) {
return move(offset);
}
if (area.contains(getStartPoint())) {
moveStartPoint(getStartPoint() + offset);
ret = true;
}
else if (area.contains(getEndPoint())) {
moveEndPoint(getEndPoint() + offset);
ret = true;
}
return ret;
}
void ArrowLinkItem::paint(QPainter *painter, const QStyleOptionGraphicsItem *option, QWidget *widget)
{
QColor penColor;
QPointF c1 = getControlItemPosFirst();
QPointF c2 = getControlItemPosSecond();
QPointF startPos = getStartPoint();
QPointF endPos = getEndPoint();
if(option->state & QStyle::State_Selected)
{
setControlPointVisible(true);
QPen pen(Qt::DotLine);
pen.setWidth(1);
pen.setColor(QColor(0, 0, 255, 125));
painter->save();
painter->setPen(pen);
painter->drawLine(c1, startPos);
painter->drawLine(c2, endPos);
painter->restore();
penColor = m_colorSelect;
}
else
{
setControlPointVisible(false);
penColor = Qt::blue;
}
if(option->state & QStyle::State_MouseOver)
{
penColor = penColor.lighter();
}
if(option->state & QStyle::State_Sunken)
{
penColor.setAlpha(210);
}
painter->setPen(QPen(penColor, 2, Qt::SolidLine, Qt::RoundCap, Qt::RoundJoin));
QPainterPath path = originalShape();
painter->drawPath(path);
qreal arrowSize = 10;
QLineF line(endPos, c2);
double angle = ::acos(line.dx() / (line.length() <= 0 ? 1 : line.length()));
if (line.dy() >= 0)
angle = (M_PI * 2) - angle;
QPointF arrowP1 = line.p1() + QPointF(sin(angle + M_PI / 3) * arrowSize,
cos(angle + M_PI / 3) * arrowSize);
QPointF arrowP2 = line.p1() + QPointF(sin(angle + M_PI - M_PI / 3) * arrowSize,
cos(angle + M_PI - M_PI / 3) * arrowSize);
QPolygonF arrowHead;
arrowHead << arrowP2 << arrowP1 << line.p1();
painter->setBrush(penColor);
painter->drawPolygon(arrowHead);
}
void C3dBoundedLine::draw(cv::Mat & M, const CWorldCamera & P, const cv::Scalar & col, const int nThickness) const
{
CHECK_MAT_INIT(M);
const optional<const C2dImagePointPx> p0 = P.projectToPx(getStartPoint());
const optional<const C2dImagePointPx> p1 = P.projectToPx(getFinishPoint());
if(p0 && p1)
cv::line(M, *p0, *p1, col, nThickness);
}
optional<const C2dBoundedLine> C3dBoundedLine::projectBounded(const CWorldCamera & P) const
{
const optional<const C2dImagePointPx> p1 = P.projectToPx(getStartPoint());
const optional<const C2dImagePointPx> p2 = P.projectToPx(getFinishPoint());
if(!p1 || !p2)
return optional<const C2dBoundedLine>();
return optional<const C2dBoundedLine>(C2dBoundedLine(*p1,*p2));
}
void hdPolyLineFigure::basicDraw(wxBufferedDC &context, hdDrawingView *view)
{
int posIdx = view->getIdx();
if(points[posIdx]->count() < 2)
{
return;
}
hdPoint start, end;
if(startTerminal)
{
startTerminal->setLinePen(linePen);
start = startTerminal->draw(context, getStartPoint(posIdx), pointAt(posIdx, 1), view);
}
else
{
start = getStartPoint(posIdx);
}
if(endTerminal)
{
endTerminal->setLinePen(linePen);
end = endTerminal->draw(context, getEndPoint(posIdx), pointAt(posIdx, pointCount(posIdx) - 2), view);
}
else
{
end = getEndPoint(posIdx);
}
context.SetPen(linePen);
for(int i = 0; i < points[posIdx]->count() - 1; i++)
{
hdPoint *p1 = (hdPoint *) points[posIdx]->getItemAt(i);
hdPoint *p2 = (hdPoint *) points[posIdx]->getItemAt(i + 1);
hdPoint copyP1 = hdPoint (*p1);
view->CalcScrolledPosition(copyP1.x, copyP1.y, ©P1.x, ©P1.y);
hdPoint copyP2 = hdPoint (*p2);
view->CalcScrolledPosition(copyP2.x, copyP2.y, ©P2.x, ©P2.y);
context.DrawLine(copyP1, copyP2);
}
}
void RArc::moveEndPoint(const RVector& pos) {
double bulge = getBulge();
// full circle: trim instead of move:
if (bulge < 1.0e-6 || bulge > 1.0e6) {
endAngle = center.getAngleTo(pos);
}
else {
*this = RArc::createFrom2PBulge(getStartPoint(), pos, bulge);
}
}
bool CapsuleShape::findRayIntersectionWithCaps(const glm::vec3& capsuleCenter, RayIntersectionInfo& intersection) const {
glm::vec3 capCenter;
getStartPoint(capCenter);
bool hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection);
getEndPoint(capCenter);
hit = findRayIntersectionWithCap(capCenter, _radius, capsuleCenter, intersection) || hit;
if (hit) {
intersection._hitShape = const_cast<CapsuleShape*>(this);
}
return hit;
}
/**
* Uses a previously set point (the start point) and calculates the distance from it to the current Point.
* Called when moving the mouse.
* @param point The new current point
* \sa getStartPoint() calculateStartEndDistance()
**/
void DkDistanceMeasure::setCurPoint(QPoint point) {
if(!snap) {
curPoint = point;
} else {
// snapping is active -> snap to 45 degree line
curPoint = performSnapping(getStartPoint(), point);
}
calculateStartEndDistance();
}
