string Bayesian::getTaxonomy(Sequence* seq) {
try {
string tax = "";
Kmer kmer(kmerSize);
flipped = false;
//get words contained in query
//getKmerString returns a string where the index in the string is hte kmer number
//and the character at that index can be converted to be the number of times that kmer was seen
string queryKmerString = kmer.getKmerString(seq->getUnaligned());
vector<int> queryKmers;
for (int i = 0; i < queryKmerString.length()-1; i++) { // the -1 is to ignore any kmer with an N in it
if (queryKmerString[i] != '!') { //this kmer is in the query
queryKmers.push_back(i);
}
}
//if user wants to test reverse compliment and its reversed use that instead
if (flip) {
if (isReversed(queryKmers)) {
flipped = true;
seq->reverseComplement();
queryKmerString = kmer.getKmerString(seq->getUnaligned());
queryKmers.clear();
for (int i = 0; i < queryKmerString.length()-1; i++) { // the -1 is to ignore any kmer with an N in it
if (queryKmerString[i] != '!') { //this kmer is in the query
queryKmers.push_back(i);
}
}
}
}
if (queryKmers.size() == 0) { m->mothurOut(seq->getName() + " is bad. It has no kmers of length " + toString(kmerSize) + "."); m->mothurOutEndLine(); simpleTax = "unknown;"; return "unknown;"; }
int index = getMostProbableTaxonomy(queryKmers);
if (m->control_pressed) { return tax; }
//bootstrap - to set confidenceScore
int numToSelect = queryKmers.size() / 8;
if (m->debug) { m->mothurOut(seq->getName() + "\t"); }
tax = bootstrapResults(queryKmers, index, numToSelect);
if (m->debug) { m->mothurOut("\n"); }
return tax;
}
catch(exception& e) {
m->errorOut(e, "Bayesian", "getTaxonomy");
exit(1);
}
}
void REllipseEntity::print(QDebug dbg) const {
dbg.nospace() << "REllipseEntity(";
REntity::print(dbg);
dbg.nospace() << ", center: " << getCenter();
dbg.nospace() << ", majorPoint: " << getMajorPoint();
dbg.nospace() << ", ratio: " << getRatio();
dbg.nospace() << ", startAngle: " << getStartAngle();
dbg.nospace() << ", endAngle: " << getEndAngle();
dbg.nospace() << ", reversed: " << isReversed() << ")";
}
void RS_Arc::calculateBorders() {
double minX = std::min(startpoint.x, endpoint.x);
double minY = std::min(startpoint.y, endpoint.y);
double maxX = std::max(startpoint.x, endpoint.x);
double maxY = std::max(startpoint.y, endpoint.y);
double a1 = !isReversed() ? data.angle1 : data.angle2;
double a2 = !isReversed() ? data.angle2 : data.angle1;
// 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 = std::min(data.center.x - data.radius, minX);
}
// check for right limit:
if (a1 > a2-1.0e-12) {
maxX = std::max(data.center.x + data.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 = std::min(data.center.y - data.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 = std::max(data.center.y + data.radius, maxY);
}
minV.set(minX, minY);
maxV.set(maxX, maxY);
}
void RArc::print(QDebug dbg) const {
dbg.nospace() << "RArc(";
RShape::print(dbg);
dbg.nospace() << ", center: " << getCenter()
<< ", radius: " << getRadius()
<< ", startAngle: " << getStartAngle()
<< ", endAngle: " << getEndAngle()
<< ", startPoint: " << getStartPoint()
<< ", endPoint: " << getEndPoint()
<< ", reversed: " << isReversed()
<< ")";
}
RigidBodyNode* MobilizedBody::CustomImpl::createRigidBodyNode(
UIndex& nextUSlot,
USquaredIndex& nextUSqSlot,
QIndex& nextQSlot) const
{
bool noX_MB = (getDefaultOutboardFrame().p() == 0 && getDefaultOutboardFrame().R() == Mat33(1));
bool noR_PF = (getDefaultInboardFrame().R() == Mat33(1));
switch (getImplementation().getImpl().getNU()) {
case 1:
INSTANTIATE_CUSTOM(1, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 2:
INSTANTIATE_CUSTOM(2, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 3:
INSTANTIATE_CUSTOM(3, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 4:
INSTANTIATE_CUSTOM(4, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 5:
INSTANTIATE_CUSTOM(5, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
case 6:
INSTANTIATE_CUSTOM(6, getImplementation(), getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(), getDefaultOutboardFrame(), isReversed(), nextUSlot, nextUSqSlot, nextQSlot)
default:
assert(!"Illegal number of degrees of freedom for custom MobilizedBody");
return 0;
}
}
/**
* Calculates the boundary box of this ellipse.
*
* @todo Fix that - the algorithm used is really bad / slow.
*/
void RS_Ellipse::calculateBorders() {
RS_DEBUG->print("RS_Ellipse::calculateBorders");
double radius1 = getMajorRadius();
double radius2 = getMinorRadius();
double angle = getAngle();
double a1 = ((!isReversed()) ? data.angle1 : data.angle2);
double a2 = ((!isReversed()) ? data.angle2 : data.angle1);
RS_Vector startpoint = getStartpoint();
RS_Vector endpoint = getEndpoint();
double minX = std::min(startpoint.x, endpoint.x);
double minY = std::min(startpoint.y, endpoint.y);
double maxX = std::max(startpoint.x, endpoint.x);
double maxY = std::max(startpoint.y, endpoint.y);
// kind of a brute force. TODO: calculation
RS_Vector vp;
double a = a1;
do {
vp.set(data.center.x + radius1 * cos(a),
data.center.y + radius2 * sin(a));
vp.rotate(data.center, angle);
minX = std::min(minX, vp.x);
minY = std::min(minY, vp.y);
maxX = std::max(maxX, vp.x);
maxY = std::max(maxY, vp.y);
a += 0.03;
} while (RS_Math::isAngleBetween(RS_Math::correctAngle(a), a1, a2, false) &&
a<4*M_PI);
minV.set(minX, minY);
maxV.set(maxX, maxY);
RS_DEBUG->print("RS_Ellipse::calculateBorders: OK");
}
/**
* @return Middle point of the entity.
*/
RS_Vector RS_Arc::getMiddlepoint() const {
double a;
RS_Vector ret;
if (isReversed()) {
a = data.angle1 - getAngleLength()/2.0;
} else {
a = data.angle1 + getAngleLength()/2.0;
}
ret.setPolar(data.radius, a);
ret+=data.center;
return ret;
}
/**
* @return Angle length in rad.
*/
double RS_Arc::getAngleLength() const {
if (isReversed()) {
if (data.angle1<data.angle2) {
return data.angle1+2*M_PI-data.angle2;
} else {
return data.angle1-data.angle2;
}
} else {
if (data.angle2<data.angle1) {
return data.angle2+2*M_PI-data.angle1;
} else {
return data.angle2-data.angle1;
}
}
}
RS_Vector RS_Arc::getNearestDist(double distance,
const RS_Vector& coord,
double* dist) {
if (data.radius<1.0e-6) {
if (dist!=NULL) {
*dist = RS_MAXDOUBLE;
}
return RS_Vector(false);
}
double a1, a2;
RS_Vector p1, p2;
double aDist = distance / data.radius;
if (isReversed()) {
a1 = data.angle1 - aDist;
a2 = data.angle2 + aDist;
} else {
a1 = data.angle1 + aDist;
a2 = data.angle2 - aDist;
}
p1.setPolar(data.radius, a1);
p1 += data.center;
p2.setPolar(data.radius, a2);
p2 += data.center;
double dist1, dist2;
RS_Vector* nearerPoint;
dist1 = p1.distanceTo(coord);
dist2 = p2.distanceTo(coord);
if (dist2<dist1) {
if (dist!=NULL) {
*dist = dist2;
}
nearerPoint = &p2;
} else {
if (dist!=NULL) {
*dist = dist1;
}
nearerPoint = &p1;
}
return *nearerPoint;
}
void REllipse::print(QDebug dbg) const {
dbg.nospace() << "REllipse(";
RShape::print(dbg);
dbg.nospace() << ", startPoint: " << getStartPoint()
<< ", endPoint: " << getEndPoint()
<< ", center: " << getCenter()
<< ", majorPoint: " << getMajorPoint()
<< ", majorRadius: " << getMajorRadius()
<< ", minorRadius: " << getMinorRadius()
<< ", ratio: " << getRatio()
<< ", startAngle: " << RMath::rad2deg(getStartParam())
<< ", endAngle: " << RMath::rad2deg(getEndParam())
<< ", full: " << isFullEllipse()
<< ", clockwise: " << isReversed()
<< ")";
}
RS_Vector RS_Ellipse::getNearestOrthTan(const RS_Vector& coord,
const RS_Line& normal,
bool onEntity )
{
if ( !coord.valid ) {
return RS_Vector(false);
}
RS_Vector direction=normal.getEndpoint() - normal.getStartpoint();
if (RS_Vector::dotP(direction,direction)< RS_TOLERANCE*RS_TOLERANCE) {
//undefined direction
return RS_Vector(false);
}
//scale to ellipse angle
direction.rotate(-getAngle());
double angle=direction.scale(RS_Vector(1.,getRatio())).angle();
direction.set(getMajorRadius()*cos(angle),getMinorRadius()*sin(angle));//relative to center
QList<RS_Vector> sol;
for(int i=0; i<2; i++) {
if(!onEntity ||
RS_Math::isAngleBetween(angle,getAngle1(),getAngle2(),isReversed())) {
if(i) {
sol.append(- direction);
} else {
sol.append(direction);
}
}
angle=RS_Math::correctAngle(angle+M_PI);
}
RS_Vector vp;
switch(sol.count()) {
case 0:
return RS_Vector(false);
case 2:
if( RS_Vector::dotP(sol[1]-getCenter(),coord-getCenter())>0.) {
vp=sol[1];
break;
}
default:
vp=sol[0];
}
return getCenter() + vp.rotate(getAngle());
}
RS_Vector RS_Arc::getNearestPointOnEntity(const RS_Vector& coord,
bool onEntity, double* dist, RS_Entity** entity) {
RS_Vector vec(false);
if (entity!=NULL) {
*entity = this;
}
double angle = (coord-data.center).angle();
if (onEntity==false || RS_Math::isAngleBetween(angle,
data.angle1, data.angle2, isReversed())) {
vec.setPolar(data.radius, angle);
vec+=data.center;
}
if (dist!=NULL) {
*dist = fabs((vec-data.center).magnitude()-data.radius);
}
return vec;
}
/**
* mirror by the axis defined by axisPoint1 and axisPoint2
*/
void RS_Ellipse::mirror(RS_Vector axisPoint1, RS_Vector axisPoint2) {
RS_Vector center=getCenter();
RS_Vector mp = center + getMajorP();
RS_Vector startpoint = getStartpoint();
RS_Vector endpoint = getEndpoint();
center.mirror(axisPoint1, axisPoint2);
mp.mirror(axisPoint1, axisPoint2);
startpoint.mirror(axisPoint1, axisPoint2);
endpoint.mirror(axisPoint1, axisPoint2);
setCenter(center);
setReversed(!isReversed());
setMajorP(mp - center);
if( std::isnormal(getAngle1()) || std::isnormal(getAngle2() ) ) {
//only reset start/end points for ellipse arcs, i.e., angle1 angle2 are not both zero
setAngle1( getEllipseAngle(startpoint));
setAngle2( getEllipseAngle(endpoint));
}
/* old version
data.majorP = mp - data.center;
double a = axisPoint1.angleTo(axisPoint2);
RS_Vector vec;
vec.setPolar(1.0, data.angle1);
vec.mirror(RS_Vector(0.0,0.0), axisPoint2-axisPoint1);
data.angle1 = vec.angle() - 2*a;
vec.setPolar(1.0, data.angle2);
vec.mirror(RS_Vector(0.0,0.0), axisPoint2-axisPoint1);
data.angle2 = vec.angle() - 2*a;
data.reversed = (!data.reversed);
*/
//calculateEndpoints();
correctAngles();//avoid extra 2.*M_PI in angles
calculateBorders();
}
/*!
* Note that this has complexity O(n^2) but should not count since
* triangulation has O(n^3).
*
* \param globalverts the global vertices vector
* \param polylist the global list of polygons
* \return zero on success, and a negative integer if some error occured.
*/
int SimplePolygon::fixAndTriangulate( DCTPVec2dvector &globalverts, simplepolygonvector &polylist )
{
Vec2d min, max;
if( getMinMax( globalverts, min, max ) )
{
// std::cerr << "Ignoring degenerate polygon..." << std::endl;
return -1;
}
// while( removeLinearPoints( globalverts, min, max ) ) { }
while( splitPolygon( globalverts, polylist, min, max ) ) { }
while( intersectPolygon( globalverts, polylist ) ) { }
if( isReversed( globalverts ) )
{
// std::cerr << "Ignoring reversed polygon." << std::endl;
return 0; // no error here...
}
// Ok, we have a valid polygon, so triangulate it.
triangulate( globalverts, polylist );
return 0;
}
/**
* Overrides drawing of subentities. This is only ever called for solid fills.
*/
void RS_Hatch::draw(RS_Painter* painter, RS_GraphicView* view, double& /*patternOffset*/) {
if (!data.solid) {
for(auto se: entities){
view->drawEntity(painter,se);
}
return;
}
//area of solid fill. Use polygon approximation, except trivial cases
QPainterPath path;
QList<QPolygon> paClosed;
QPolygon pa;
// QPolygon jp; // jump points
// loops:
if (needOptimization==true) {
for(auto l: entities){
if (l->rtti()==RS2::EntityContainer) {
RS_EntityContainer* loop = (RS_EntityContainer*)l;
loop->optimizeContours();
}
}
needOptimization = false;
}
// loops:
for(auto l: entities){
l->setLayer(getLayer());
if (l->rtti()==RS2::EntityContainer) {
RS_EntityContainer* loop = (RS_EntityContainer*)l;
// edges:
for(auto e: *loop){
e->setLayer(getLayer());
switch (e->rtti()) {
case RS2::EntityLine: {
QPoint pt1(RS_Math::round(view->toGuiX(e->getStartpoint().x)),
RS_Math::round(view->toGuiY(e->getStartpoint().y)));
QPoint pt2(RS_Math::round(view->toGuiX(e->getEndpoint().x)),
RS_Math::round(view->toGuiY(e->getEndpoint().y)));
// if (! (pa.size()>0 && (pa.last() - pt1).manhattanLength()<=2)) {
// jp<<pt1;
// }
if(pa.size() && (pa.last()-pt1).manhattanLength()>=1)
pa<<pt1;
pa<<pt2;
}
break;
case RS2::EntityArc: {
// QPoint pt1(RS_Math::round(view->toGuiX(e->getStartpoint().x)),
// RS_Math::round(view->toGuiY(e->getStartpoint().y)));
// if (! (pa.size()>0 && (pa.last() - pt1).manhattanLength()<=2)) {
// jp<<pt1;
// }
QPolygon pa2;
RS_Arc* arc=static_cast<RS_Arc*>(e);
painter->createArc(pa2, view->toGui(arc->getCenter()),
view->toGuiDX(arc->getRadius())
,arc->getAngle1(),arc->getAngle2(),arc->isReversed());
if(pa.size() &&pa2.size()&&(pa.last()-pa2.first()).manhattanLength()<1)
pa2.remove(0,1);
pa<<pa2;
}
break;
case RS2::EntityCircle: {
RS_Circle* circle = static_cast<RS_Circle*>(e);
// QPoint pt1(RS_Math::round(view->toGuiX(circle->getCenter().x+circle->getRadius())),
// RS_Math::round(view->toGuiY(circle->getCenter().y)));
// if (! (pa.size()>0 && (pa.last() - pt1).manhattanLength()<=2)) {
// jp<<pt1;
// }
RS_Vector c=view->toGui(circle->getCenter());
double r=view->toGuiDX(circle->getRadius());
#if QT_VERSION >= 0x040400
path.addEllipse(QPoint(c.x,c.y),r,r);
#else
path.addEllipse(c.x - r, c.y + r, 2.*r, 2.*r);
// QPolygon pa2;
// painter->createArc(pa2, view->toGui(circle->getCenter()),
// view->toGuiDX(circle->getRadius()),
// 0.0,
// 2*M_PI,
// false);
// pa<<pa2;
#endif
}
break;
case RS2::EntityEllipse:
if(static_cast<RS_Ellipse*>(e)->isArc()) {
QPolygon pa2;
auto ellipse=static_cast<RS_Ellipse*>(e);
painter->createEllipse(pa2,
view->toGui(ellipse->getCenter()),
view->toGuiDX(ellipse->getMajorRadius()),
view->toGuiDX(ellipse->getMinorRadius()),
ellipse->getAngle()
,ellipse->getAngle1(),ellipse->getAngle2(),ellipse->isReversed()
);
// qDebug()<<"ellipse: "<<ellipse->getCenter().x<<","<<ellipse->getCenter().y;
// qDebug()<<"ellipse: pa2.size()="<<pa2.size();
// qDebug()<<"ellipse: pa2="<<pa2;
if(pa.size() && pa2.size()&&(pa.last()-pa2.first()).manhattanLength()<1)
pa2.remove(0,1);
pa<<pa2;
}else{
QPolygon pa2;
auto ellipse=static_cast<RS_Ellipse*>(e);
painter->createEllipse(pa2,
view->toGui(ellipse->getCenter()),
view->toGuiDX(ellipse->getMajorRadius()),
view->toGuiDX(ellipse->getMinorRadius()),
ellipse->getAngle(),
ellipse->getAngle1(), ellipse->getAngle2(),
ellipse->isReversed()
);
path.addPolygon(pa2);
}
break;
default:
break;
}
// qDebug()<<"pa="<<pa;
if( pa.size()>2 && pa.first() == pa.last()) {
paClosed<<pa;
pa.clear();
}
}
}
}
if(pa.size()>2){
pa<<pa.first();
paClosed<<pa;
}
for(auto& p:paClosed){
path.addPolygon(p);
}
//bug#474, restore brush after solid fill
const QBrush brush(painter->brush());
const RS_Pen pen=painter->getPen();
painter->setBrush(pen.getColor());
painter->disablePen();
painter->drawPath(path);
painter->setBrush(brush);
painter->setPen(pen);
}
RS_Vector RS_Ellipse::prepareTrim(const RS_Vector& trimCoord,
const RS_VectorSolutions& trimSol) {
//special trimming for ellipse arc
if( ! trimSol.hasValid() ) return (RS_Vector(false));
if( trimSol.getNumber() == 1 ) return (trimSol.get(0));
double am=getEllipseAngle(trimCoord);
QList<double> ias;
double ia(0.),ia2(0.);
RS_Vector is,is2;
for(int ii=0; ii<trimSol.getNumber(); ii++) { //find closest according ellipse angle
ias.append(getEllipseAngle(trimSol.get(ii)));
if( !ii || fabs( remainder( ias[ii] - am, 2*M_PI)) < fabs( remainder( ia -am, 2*M_PI)) ) {
ia = ias[ii];
is = trimSol.get(ii);
}
}
qSort(ias.begin(),ias.end());
for(int ii=0; ii<trimSol.getNumber(); ii++) { //find segment to enclude trimCoord
if ( ! RS_Math::isSameDirection(ia,ias[ii],RS_TOLERANCE)) continue;
if( RS_Math::isAngleBetween(am,ias[(ii+trimSol.getNumber()-1)% trimSol.getNumber()],ia,false)) {
ia2=ias[(ii+trimSol.getNumber()-1)% trimSol.getNumber()];
} else {
ia2=ias[(ii+1)% trimSol.getNumber()];
}
break;
}
for(int ii=0; ii<trimSol.getNumber(); ii++) { //find segment to enclude trimCoord
if ( ! RS_Math::isSameDirection(ia2,getEllipseAngle(trimSol.get(ii)),RS_TOLERANCE)) continue;
is2=trimSol.get(ii);
break;
}
if(RS_Math::isSameDirection(getAngle1(),getAngle2(),RS_TOLERANCE_ANGLE)
|| RS_Math::isSameDirection(ia2,ia,RS_TOLERANCE) ) {
//whole ellipse
if( !RS_Math::isAngleBetween(am,ia,ia2,isReversed())) {
std::swap(ia,ia2);
std::swap(is,is2);
}
setAngle1(ia);
setAngle2(ia2);
double da1=fabs(remainder(getAngle1()-am,2*M_PI));
double da2=fabs(remainder(getAngle2()-am,2*M_PI));
if(da2<da1) {
std::swap(is,is2);
}
} else {
double dia=fabs(remainder(ia-am,2*M_PI));
double dia2=fabs(remainder(ia2-am,2*M_PI));
double ai_min=std::min(dia,dia2);
double da1=fabs(remainder(getAngle1()-am,2*M_PI));
double da2=fabs(remainder(getAngle2()-am,2*M_PI));
double da_min=std::min(da1,da2);
if( da_min < ai_min ) {
//trimming one end of arc
bool irev= RS_Math::isAngleBetween(am,ia2,ia, isReversed()) ;
if ( RS_Math::isAngleBetween(ia,getAngle1(),getAngle2(), isReversed()) &&
RS_Math::isAngleBetween(ia2,getAngle1(),getAngle2(), isReversed()) ) { //
if(irev) {
setAngle2(ia);
setAngle1(ia2);
} else {
setAngle1(ia);
setAngle2(ia2);
}
da1=fabs(remainder(getAngle1()-am,2*M_PI));
da2=fabs(remainder(getAngle2()-am,2*M_PI));
}
if( ((da1 < da2) && (RS_Math::isAngleBetween(ia2,ia,getAngle1(),isReversed()))) ||
((da1 > da2) && (RS_Math::isAngleBetween(ia2,getAngle2(),ia,isReversed())))
) {
std::swap(is,is2);
//std::cout<<"reset: angle1="<<getAngle1()<<" angle2="<<getAngle2()<<" am="<< am<<" is="<<getEllipseAngle(is)<<" ia2="<<ia2<<std::endl;
}
} else {
//choose intersection as new end
if( dia > dia2) {
std::swap(is,is2);
std::swap(ia,ia2);
}
if(RS_Math::isAngleBetween(ia,getAngle1(),getAngle2(),isReversed())) {
if(RS_Math::isAngleBetween(am,getAngle1(),ia,isReversed())) {
setAngle2(ia);
} else {
setAngle1(ia);
}
}
}
}
return is;
}
//------------------------------------------------------------------------------
// serialize() -- print functions
//------------------------------------------------------------------------------
std::ostream& Field::serialize(std::ostream& sout, const int i, const bool slotsOnly) const
{
int j = 0;
if ( !slotsOnly ) {
sout << "( " << getFormName() << std::endl;
j = 4;
}
BaseClass::serialize(sout,i+j,true);
if (line() > 0 && column() > 0) {
indent(sout,i+j);
sout << "position: [ " << line() << " " << column() << " ]" << std::endl;
}
if ( width() > 0) {
indent(sout,i+j);
sout << "width: " << (unsigned int)width() << std::endl;
}
if ( isHighLighted() ) {
indent(sout,i+j);
sout << "highLight: " << isHighLighted() << std::endl;
}
if ( isUnderlined() ) {
indent(sout,i+j);
sout << "underline: " << isUnderlined() << std::endl;
}
if ( isReversed() ) {
indent(sout,i+j);
sout << "reversed: " << isReversed() << std::endl;
}
if ( isVertical() ) {
indent(sout,i+j);
sout << "vertical: " << isVertical() << std::endl;
}
if ( areBracketsOn() ) {
indent(sout,i+j);
sout << "brackets: " << areBracketsOn() << std::endl;
}
indent(sout,i+j);
sout << "justification: ";
switch (jmode) {
case Basic::String::NONE : sout << "\"none\""; break;
case Basic::String::LEFT : sout << "\"left\""; break;
case Basic::String::CENTER : sout << "\"center\""; break;
case Basic::String::RIGHT : sout << "\"right\""; break;
}
sout << std::endl;
if ( !slotsOnly ) {
indent(sout,i);
sout << ")" << std::endl;
}
return sout;
}
//------------------------------------------------------------------------------
// drawFunc -- draw this text field
//------------------------------------------------------------------------------
void Field::drawFunc()
{
// Get a pointer to the current display
BasicGL::Display* dsp = getDisplay();
if (dsp == 0) return;
// ---
// When our container is also a Field, get a pointer to it.
// ---
BasicGL::Field* parent = 0;
if (container() != 0) {
BasicGL::Field* fp = dynamic_cast<BasicGL::Field*>(container());
if (fp != 0) parent = fp;
}
// ---
// If we don't have a position, try to get one from our container
// ---
int ll = line();
int cc = column();
if (ll == 0 && parent != 0) {
ll = parent->line();
cc = parent->column();
}
// ---
// Select the correct font based on font name if there is one, and if not, then do it normally
// ---
if (fontName != 0) dsp->selectFont(isReversed(), isUnderlined(), dsp->getFont(fontName->getString()));
else dsp->selectFont(isReversed(), isUnderlined());
// ---
// Set the color
// ---
bool restoreColor = false;
osg::Vec4 ocolor = dsp->getCurrentColor();
// only use default colors if we aren't inheriting our container's colors
if (!isInheritColor()) {
if (getColorName() == 0 && getColor() == 0) {
const Basic::Color* cc = 0;
if (isHighLighted()) cc = dsp->getHighlightColor();
else cc = dsp->getNormColor();
if (cc != 0) {
const osg::Vec4* p = cc->getRGBA();
dsp->setColor(*p);
restoreColor = true;
}
}
}
// ---
// draw the string
// ---
if (str.len() > 0) {
// Draw the text string
const char* sp = str;
if (ll > 0 && cc > 0)
dsp->outputTextLC(ll, cc, sp, int(width()), isVertical());
else
dsp->outputText(sp, int(width()), isVertical());
}
// ---
// draw the brackets
// ---
if (areBracketsOn() && ll > 0 && cc > 0) {
if (isVertical()) {
// Position for vertical text
dsp->drawLeftBracket(ll-1, cc);
dsp->drawRightBracket(ll+int(width()), cc);
}
else {
// Position for normal text
dsp->drawLeftBracket(ll, cc-1);
dsp->drawRightBracket(ll, cc+int(width()));
}
}
// ---
// If we used default colors, restore the old value
// ---
if (restoreColor) dsp->setColor(ocolor);
}
void CKJVPassageNavigator::startRelativeMode(TPhraseTag tagStart, TPhraseTag tagPassage)
{
startRelativeMode(tagStart, isReversed(), tagPassage);
}
/////////////////////////////////////////////////////////
// These probably don't belong here.
#include "MobilizedBodyImpl.h"
RigidBodyNode* MobilizedBody::PinImpl::createRigidBodyNode(
UIndex& nextUSlot,
USquaredIndex& nextUSqSlot,
QIndex& nextQSlot) const
{
INSTANTIATE(RBNodeTorsion,
getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(),getDefaultOutboardFrame(),
isReversed(),
nextUSlot,nextUSqSlot,nextQSlot)
}
RigidBodyNode* MobilizedBody::SliderImpl::createRigidBodyNode(
UIndex& nextUSlot,
USquaredIndex& nextUSqSlot,
QIndex& nextQSlot) const
{
INSTANTIATE(RBNodeSlider,
getDefaultRigidBodyMassProperties(),
getDefaultInboardFrame(),getDefaultOutboardFrame(),
isReversed(),
nextUSlot,nextUSqSlot,nextQSlot)
}
/**
* Calculates the boundary box of this ellipse.
*/
void RS_Ellipse::calculateBorders() {
RS_DEBUG->print("RS_Ellipse::calculateBorders");
double radius1 = getMajorRadius();
double radius2 = getMinorRadius();
double angle = getAngle();
//double a1 = ((!isReversed()) ? data.angle1 : data.angle2);
//double a2 = ((!isReversed()) ? data.angle2 : data.angle1);
RS_Vector startpoint = getStartpoint();
RS_Vector endpoint = getEndpoint();
double minX = std::min(startpoint.x, endpoint.x);
double minY = std::min(startpoint.y, endpoint.y);
double maxX = std::max(startpoint.x, endpoint.x);
double maxY = std::max(startpoint.y, endpoint.y);
RS_Vector vp;
// kind of a brute force. TODO: exact calculation
// double a = a1;
// do {
// vp.set(data.center.x + radius1 * cos(a),
// data.center.y + radius2 * sin(a));
// vp.rotate(data.center, angle);
//
// minX = std::min(minX, vp.x);
// minY = std::min(minY, vp.y);
// maxX = std::max(maxX, vp.x);
// maxY = std::max(maxY, vp.y);
//
// a += 0.03;
// } while (RS_Math::isAngleBetween(RS_Math::correctAngle(a), a1, a2, false) &&
// a<4*M_PI);
// std::cout<<"a1="<<a1<<"\ta2="<<a2<<std::endl<<"Old algorithm:\nminX="<<minX<<"\tmaxX="<<maxX<<"\nminY="<<minY<<"\tmaxY="<<maxY<<std::endl;
// Exact algorithm, based on rotation:
// ( r1*cos(a), r2*sin(a)) rotated by angle to
// (r1*cos(a)*cos(angle)-r2*sin(a)*sin(angle),r1*cos(a)*sin(angle)+r2*sin(a)*cos(angle))
// both coordinates can be further reorganized to the form rr*cos(a+ theta),
// with rr and theta angle defined by the coordinates given above
double amin,amax;
// x range
vp.set(radius1*cos(angle),radius2*sin(angle));
amin=RS_Math::correctAngle(getAngle1()+vp.angle()); // to the range of 0 to 2*M_PI
amax=RS_Math::correctAngle(getAngle2()+vp.angle()); // to the range of 0 to 2*M_PI
if( RS_Math::isAngleBetween(M_PI,amin,amax,isReversed()) ) {
//if( (amin<=M_PI && delta_a >= M_PI - amin) || (amin > M_PI && delta_a >= 3*M_PI - amin)) {
minX= data.center.x-vp.magnitude();
}
// else
// minX=data.center.x +vp.magnitude()*std::min(cos(amin),cos(amin+delta_a));
if( RS_Math::isAngleBetween(2.*M_PI,amin,amax,isReversed()) ) {
//if( delta_a >= 2*M_PI - amin ) {
maxX= data.center.x+vp.magnitude();
}// else
// maxX= data.center.x+vp.magnitude()*std::max(cos(amin),cos(amin+delta_a));
// y range
vp.set(radius1*sin(angle),-radius2*cos(angle));
amin=RS_Math::correctAngle(getAngle1()+vp.angle()); // to the range of 0 to 2*M_PI
amax=RS_Math::correctAngle(getAngle2()+vp.angle()); // to the range of 0 to 2*M_PI
if( RS_Math::isAngleBetween(M_PI,amin,amax,isReversed()) ) {
//if( (amin<=M_PI &&delta_a >= M_PI - amin) || (amin > M_PI && delta_a >= 3*M_PI - amin)) {
minY= data.center.y-vp.magnitude();
}// else
// minY=data.center.y +vp.magnitude()*std::min(cos(amin),cos(amin+delta_a));
if( RS_Math::isAngleBetween(2.*M_PI,amin,amax,isReversed()) ) {
//if( delta_a >= 2*M_PI - amin ) {
maxY= data.center.y+vp.magnitude();
}
// else
// maxY= data.center.y+vp.magnitude()*std::max(cos(amin),cos(amin+delta_a));
//std::cout<<"New algorithm:\nminX="<<minX<<"\tmaxX="<<maxX<<"\nminY="<<minY<<"\tmaxY="<<maxY<<std::endl;
minV.set(minX, minY);
maxV.set(maxX, maxY);
RS_DEBUG->print("RS_Ellipse::calculateBorders: OK");
}
void RS_Ellipse::draw(RS_Painter* painter, RS_GraphicView* view, double patternOffset) {
if (painter==NULL || view==NULL) {
return;
}
if (getPen().getLineType()==RS2::SolidLine ||
isSelected() ||
view->getDrawingMode()==RS2::ModePreview) {
painter->drawEllipse(view->toGui(getCenter()),
getMajorRadius() * view->getFactor().x,
getMinorRadius() * view->getFactor().x,
getAngle(),
getAngle1(), getAngle2(),
isReversed());
} else {
double styleFactor = getStyleFactor(view);
if (styleFactor<0.0) {
painter->drawEllipse(view->toGui(getCenter()),
getMajorRadius() * view->getFactor().x,
getMinorRadius() * view->getFactor().x,
getAngle(),
getAngle1(), getAngle2(),
isReversed());
return;
}
// Pattern:
RS_LineTypePattern* pat;
if (isSelected()) {
pat = &patternSelected;
} else {
pat = view->getPattern(getPen().getLineType());
}
if (pat==NULL) {
return;
}
// Pen to draw pattern is always solid:
RS_Pen pen = painter->getPen();
pen.setLineType(RS2::SolidLine);
painter->setPen(pen);
double* da; // array of distances in x.
int i; // index counter
double length = getAngleLength();
// create pattern:
da = new double[pat->num];
double tot=0.0;
i=0;
bool done = false;
double curA = getAngle1();
double curR;
RS_Vector cp = view->toGui(getCenter());
double r1 = getMajorRadius() * view->getFactor().x;
double r2 = getMinorRadius() * view->getFactor().x;
do {
curR = sqrt(RS_Math::pow(getMinorRadius()*cos(curA), 2.0)
+ RS_Math::pow(getMajorRadius()*sin(curA), 2.0));
if (curR>1.0e-6) {
da[i] = fabs(pat->pattern[i] * styleFactor) / curR;
if (pat->pattern[i] * styleFactor > 0.0) {
if (tot+fabs(da[i])<length) {
painter->drawEllipse(cp,
r1, r2,
getAngle(),
curA,
curA + da[i],
false);
} else {
painter->drawEllipse(cp,
r1, r2,
getAngle(),
curA,
getAngle2(),
false);
}
}
}
curA+=da[i];
tot+=fabs(da[i]);
done=tot>length;
i++;
if (i>=pat->num) {
i=0;
}
} while(!done);
delete[] da;
}
}
RS_Vector RS_Ellipse::getNearestPointOnEntity(const RS_Vector& coord,
bool onEntity, double* dist, RS_Entity** entity)
{
RS_DEBUG->print("RS_Ellipse::getNearestPointOnEntity");
RS_Vector ret(false);
if( ! coord.valid ) {
if ( dist != NULL ) *dist=RS_MAXDOUBLE;
return ret;
}
if (entity!=NULL) {
*entity = this;
}
ret=coord;
ret.move(-getCenter());
ret.rotate(-getAngle());
double x=ret.x,y=ret.y;
double a=getMajorRadius();
double b=getMinorRadius();
//std::cout<<"(a= "<<a<<" b= "<<b<<" x= "<<x<<" y= "<<y<<" )\n";
//std::cout<<"finding minimum for ("<<x<<"-"<<a<<"*cos(t))^2+("<<y<<"-"<<b<<"*sin(t))^2\n";
double twoa2b2=2*(a*a-b*b);
double twoax=2*a*x;
double twoby=2*b*y;
double a0=twoa2b2*twoa2b2;
double ce[4];
double roots[4];
unsigned int counts=0;
//need to handle a=b
if(a0 > RS_TOLERANCE*RS_TOLERANCE ) { // a != b , ellipse
ce[0]=-2.*twoax/twoa2b2;
ce[1]= (twoax*twoax+twoby*twoby)/a0-1.;
ce[2]= - ce[0];
ce[3]= -twoax*twoax/a0;
//std::cout<<"1::find cosine, variable c, solve(c^4 +("<<ce[0]<<")*c^3+("<<ce[1]<<")*c^2+("<<ce[2]<<")*c+("<<ce[3]<<")=0,c)\n";
counts=RS_Math::quarticSolver(ce,roots);
} else {//a=b, quadratic equation for circle
counts=2;
a0=twoby/twoax;
roots[0]=sqrt(1./(1.+a0*a0));
roots[1]=-roots[0];
}
if(!counts) {
//this should not happen
std::cout<<"(a= "<<a<<" b= "<<b<<" x= "<<x<<" y= "<<y<<" )\n";
std::cout<<"finding minimum for ("<<x<<"-"<<a<<"*cos(t))^2+("<<y<<"-"<<b<<"*sin(t))^2\n";
std::cout<<"2::find cosine, variable c, solve(c^4 +("<<ce[0]<<")*c^3+("<<ce[1]<<")*c^2+("<<ce[2]<<")*c+("<<ce[3]<<")=0,c)\n";
std::cout<<ce[0]<<' '<<ce[1]<<' '<<ce[2]<<' '<<ce[3]<<std::endl;
std::cerr<<"RS_Math::RS_Ellipse::getNearestPointOnEntity() finds no root from quartic, this should not happen\n";
return RS_Vector(coord); // better not to return invalid: return RS_Vector(false);
}
RS_Vector vp2(false);
double d(RS_MAXDOUBLE),d2,s,dDistance(RS_MAXDOUBLE);
//double ea;
for(unsigned int i=0; i<counts; i++) {
//I don't understand the reason yet, but I can do without checking whether sine/cosine are valid
//if ( fabs(roots[i])>1.) continue;
s=twoby*roots[i]/(twoax-twoa2b2*roots[i]); //sine
//if (fabs(s) > 1. ) continue;
d2=twoa2b2+(twoax-2.*roots[i]*twoa2b2)*roots[i]+twoby*s;
if (d2<0) continue; // fartherest
RS_Vector vp3;
vp3.set(a*roots[i],b*s);
d=vp3.distanceTo(ret);
// std::cout<<i<<" Checking: cos= "<<roots[i]<<" sin= "<<s<<" angle= "<<atan2(roots[i],s)<<" ds2= "<<d<<" d="<<d2<<std::endl;
if( vp2.valid && d>dDistance) continue;
vp2=vp3;
dDistance=d;
// ea=atan2(roots[i],s);
}
if( ! vp2.valid ) {
//this should not happen
std::cout<<ce[0]<<' '<<ce[1]<<' '<<ce[2]<<' '<<ce[3]<<std::endl;
std::cout<<"(x,y)=( "<<x<<" , "<<y<<" ) a= "<<a<<" b= "<<b<<" sine= "<<s<<" d2= "<<d2<<" dist= "<<d<<std::endl;
std::cout<<"RS_Ellipse::getNearestPointOnEntity() finds no minimum, this should not happen\n";
}
if (dist!=NULL) {
*dist = dDistance;
}
vp2.rotate(getAngle());
vp2.move(getCenter());
ret=vp2;
if (onEntity) {
if (!RS_Math::isAngleBetween(getEllipseAngle(ret), getAngle1(), getAngle2(), isReversed())) { // not on entity, use the nearest endpoint
//std::cout<<"not on ellipse, ( "<<getAngle1()<<" "<<getEllipseAngle(ret)<<" "<<getAngle2()<<" ) reversed= "<<isReversed()<<"\n";
ret=getNearestEndpoint(coord,dist);
}
}
if(! ret.valid) {
std::cout<<"RS_Ellipse::getNearestOnEntity() returns invalid by mistake. This should not happen!"<<std::endl;
}
return ret;
}
/**
* Overrides drawing of subentities. This is only ever called for solid fills.
*/
void RS_Hatch::draw(RS_Painter* painter, RS_GraphicView* view, double& /*patternOffset*/) {
if (!data.solid) {
for (RS_Entity* se=firstEntity();
se!=NULL;
se = nextEntity()) {
view->drawEntity(painter,se);
}
return;
}
QPainterPath path;
QList<QPolygon> paClosed;
QPolygon pa;
// QPolygon jp; // jump points
// loops:
if (needOptimization==true) {
for (RS_Entity* l=firstEntity(RS2::ResolveNone);
l!=NULL;
l=nextEntity(RS2::ResolveNone)) {
if (l->rtti()==RS2::EntityContainer) {
RS_EntityContainer* loop = (RS_EntityContainer*)l;
loop->optimizeContours();
}
}
needOptimization = false;
}
// loops:
for (RS_Entity* l=firstEntity(RS2::ResolveNone);
l!=NULL;
l=nextEntity(RS2::ResolveNone)) {
l->setLayer(getLayer());
if (l->rtti()==RS2::EntityContainer) {
RS_EntityContainer* loop = (RS_EntityContainer*)l;
// edges:
for (RS_Entity* e=loop->firstEntity(RS2::ResolveNone);
e!=NULL;
e=loop->nextEntity(RS2::ResolveNone)) {
e->setLayer(getLayer());
switch (e->rtti()) {
case RS2::EntityLine: {
QPoint pt1(RS_Math::round(view->toGuiX(e->getStartpoint().x)),
RS_Math::round(view->toGuiY(e->getStartpoint().y)));
QPoint pt2(RS_Math::round(view->toGuiX(e->getEndpoint().x)),
RS_Math::round(view->toGuiY(e->getEndpoint().y)));
// if (! (pa.size()>0 && (pa.last() - pt1).manhattanLength()<=2)) {
// jp<<pt1;
// }
pa<<pt1<<pt2;
}
break;
case RS2::EntityArc: {
// QPoint pt1(RS_Math::round(view->toGuiX(e->getStartpoint().x)),
// RS_Math::round(view->toGuiY(e->getStartpoint().y)));
// if (! (pa.size()>0 && (pa.last() - pt1).manhattanLength()<=2)) {
// jp<<pt1;
// }
QPolygon pa2;
RS_Arc* arc=static_cast<RS_Arc*>(e);
painter->createArc(pa2, view->toGui(arc->getCenter()),
view->toGuiDX(arc->getRadius()),
arc->getAngle1(),
arc->getAngle2(),
arc->isReversed());
pa<<pa2;
}
break;
case RS2::EntityCircle: {
RS_Circle* circle = static_cast<RS_Circle*>(e);
// QPoint pt1(RS_Math::round(view->toGuiX(circle->getCenter().x+circle->getRadius())),
// RS_Math::round(view->toGuiY(circle->getCenter().y)));
// if (! (pa.size()>0 && (pa.last() - pt1).manhattanLength()<=2)) {
// jp<<pt1;
// }
RS_Vector c=view->toGui(circle->getCenter());
double r=view->toGuiDX(circle->getRadius());
#if QT_VERSION >= 0x040400
path.addEllipse(QPoint(c.x,c.y),r,r);
#else
path.addEllipse(c.x - r, c.y + r, 2.*r, 2.*r);
// QPolygon pa2;
// painter->createArc(pa2, view->toGui(circle->getCenter()),
// view->toGuiDX(circle->getRadius()),
// 0.0,
// 2*M_PI,
// false);
// pa<<pa2;
#endif
}
break;
case RS2::EntityEllipse:
if(static_cast<RS_Ellipse*>(e)->isArc()) {
QPolygon pa2;
auto ellipse=static_cast<RS_Ellipse*>(e);
painter->createEllipse(pa2,
view->toGui(ellipse->getCenter()),
view->toGuiDX(ellipse->getMajorRadius()),
view->toGuiDX(ellipse->getMinorRadius()),
ellipse->getAngle(),
ellipse->getAngle1(), ellipse->getAngle2(),
ellipse->isReversed()
);
pa<<pa2;
}else{
QPolygon pa2;
auto ellipse=static_cast<RS_Ellipse*>(e);
painter->createEllipse(pa2,
view->toGui(ellipse->getCenter()),
view->toGuiDX(ellipse->getMajorRadius()),
view->toGuiDX(ellipse->getMinorRadius()),
ellipse->getAngle(),
ellipse->getAngle1(), ellipse->getAngle2(),
ellipse->isReversed()
);
path.addPolygon(pa2);
}
break;
default:
break;
}
if( pa.size()>2 && pa.first() == pa.last()) {
paClosed<<pa;
pa.clear();
}
}
}
}
if(pa.size()>2){
pa<<pa.first();
paClosed<<pa;
}
for(int i=0;i<paClosed.size();i++){
path.addPolygon(paClosed.at(i));
}
painter->setBrush(painter->getPen().getColor());
painter->disablePen();
painter->drawPath(path);
// pa<<jp;
// painter->setBrush(painter->getPen().getColor());
// painter->disablePen();
// painter->drawPolygon(pa);
}
// Force a read/poll of the temperature pot and return the value sensed [0,255] (cold to hot).
// Potentially expensive/slow.
// This value has some hysteresis applied to reduce noise.
// Not thread-safe nor usable within ISRs (Interrupt Service Routines).
uint8_t SensorTemperaturePot::read()
{
// No need to wait for voltage to stabilise as pot top end directly driven by IO_POWER_UP.
OTV0P2BASE::power_intermittent_peripherals_enable(false);
const uint16_t tpRaw = OTV0P2BASE::analogueNoiseReducedRead(V0p2_PIN_TEMP_POT_AIN, DEFAULT); // Vcc reference.
OTV0P2BASE::power_intermittent_peripherals_disable();
const bool reverse = isReversed();
const uint16_t tp = reverse ? (TEMP_POT_RAW_MAX - tpRaw) : tpRaw;
// Capture entropy from changed LS bits.
if((uint8_t)tp != (uint8_t)raw) { ::OTV0P2BASE::addEntropyToPool((uint8_t)tp, 0); } // Claim zero entropy as may be forced by Eve.
// Capture reduced-noise value with a little hysteresis.
// Only update the value if changed significantly.
const uint8_t oldValue = value;
const uint8_t shifted = tp >> 2;
if(((shifted > oldValue) && (shifted - oldValue >= RN_HYST)) ||
((shifted < oldValue) && (oldValue - shifted >= RN_HYST)))
{
const uint8_t rn = (uint8_t) shifted;
// Atomically store reduced-noise normalised value.
value = rn;
// Smart responses to adjustment/movement of temperature pot.
// Possible to get reasonable functionality without using MODE button.
//
// Ignore first reading which might otherwise cause spurious mode change, etc.
if((uint16_t)~0U != (uint16_t)raw) // Ignore if raw not yet set for the first time.
{
const uint8_t minS = minExpected >> 2;
const uint8_t maxS = maxExpected >> 2;
// Compute low end stop threshold avoiding overflow.
const uint8_t realMinScaled = reverse ? maxS : minS;
const uint8_t loEndStop = (realMinScaled >= 255 - RN_FRBO) ? realMinScaled : (realMinScaled + RN_FRBO);
// Compute high end stop threshold avoiding underflow.
const uint8_t realMaxScaled = reverse ? minS : maxS;
const uint8_t hiEndStop = (realMaxScaled < RN_FRBO) ? realMaxScaled : (realMaxScaled - RN_FRBO);
// Force FROST mode when dial turned right down to bottom.
if(rn < loEndStop) { if(NULL != warmModeCallback) { warmModeCallback(false); } }
// Start BAKE mode when dial turned right up to top.
else if(rn > hiEndStop) { if(NULL != bakeStartCallback) { bakeStartCallback(true); } }
// Cancel BAKE mode when dial/temperature turned down.
else if(rn < oldValue) { if(NULL != bakeStartCallback) { bakeStartCallback(false); } }
// Force WARM mode when dial/temperature turned up.
else if(rn > oldValue) { if(NULL != warmModeCallback) { warmModeCallback(true); } }
// Report that the user operated the pot, ie part of the manual UI.
// Do this regardless of whether a specific mode change was invoked.
if(NULL != occCallback) { occCallback(); }
}
}
#if 0 && defined(DEBUG)
DEBUG_SERIAL_PRINT_FLASHSTRING("Temp pot: ");
DEBUG_SERIAL_PRINT(tp);
DEBUG_SERIAL_PRINT_FLASHSTRING(", rn: ");
DEBUG_SERIAL_PRINT(tempPotReducedNoise);
DEBUG_SERIAL_PRINTLN();
#endif
// Store new raw value last.
raw = tp;
// Return noise-reduced value.
return(value);
}
void RS_ActionDrawArc::commandEvent(RS_CommandEvent* e) {
QString c = e->getCommand().toLower();
if (RS_COMMANDS->checkCommand("help", c)) {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(msgAvailableCommands()
+ getAvailableCommands().join(", "));
}
return;
}
if (RS_COMMANDS->checkCommand("reversed", c)) {
e->accept();
setReversed(!isReversed());
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->requestOptions(this, true, true);
}
return;
}
switch (getStatus()) {
case SetRadius: {
bool ok;
double r = RS_Math::eval(c, &ok);
if (ok) {
data.radius = r;
setStatus(SetAngle1);
e->accept();
} else {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(tr("Not a valid expression"));
}
}
}
break;
case SetAngle1: {
bool ok;
double a = RS_Math::eval(c, &ok);
if (ok) {
data.angle1 = RS_Math::deg2rad(a);
e->accept();
setStatus(SetAngle2);
} else {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(tr("Not a valid expression"));
}
}
}
break;
case SetAngle2: {
if (RS_COMMANDS->checkCommand("angle", c)) {
setStatus(SetIncAngle);
} else if (RS_COMMANDS->checkCommand("chord length", c)) {
setStatus(SetChordLength);
} else {
bool ok;
double a = RS_Math::eval(c, &ok);
if (ok) {
data.angle2 = RS_Math::deg2rad(a);
e->accept();
trigger();
} else {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(tr("Not a valid expression"));
}
}
}
}
break;
case SetIncAngle: {
bool ok;
double a = RS_Math::eval(c, &ok);
if (ok) {
data.angle2 = data.angle1 + RS_Math::deg2rad(a);
e->accept();
trigger();
} else {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(tr("Not a valid expression"));
}
}
}
break;
case SetChordLength: {
bool ok;
double l = RS_Math::eval(c, &ok);
if (ok) {
if (fabs(l/(2*data.radius))<=1.0) {
data.angle2 = data.angle1 + asin(l/(2*data.radius)) * 2;
trigger();
} else {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(
tr("Not a valid chord length"));
}
}
e->accept();
} else {
if (RS_DIALOGFACTORY!=NULL) {
RS_DIALOGFACTORY->commandMessage(tr("Not a valid expression"));
}
}
}
break;
default:
break;
}
}
void RS_Arc::draw(RS_Painter* painter, RS_GraphicView* view,
double /*patternOffset*/) {
if (painter==NULL || view==NULL) {
return;
}
//double styleFactor = getStyleFactor();
// simple style-less lines
if (getPen().getLineType()==RS2::SolidLine ||
isSelected() ||
view->getDrawingMode()==RS2::ModePreview) {
painter->drawArc(view->toGui(getCenter()),
getRadius() * view->getFactor().x,
getAngle1(), getAngle2(),
isReversed());
} else {
double styleFactor = getStyleFactor(view);
if (styleFactor<0.0) {
painter->drawArc(view->toGui(getCenter()),
getRadius() * view->getFactor().x,
getAngle1(), getAngle2(),
isReversed());
return;
}
// Pattern:
RS_LineTypePattern* pat;
if (isSelected()) {
pat = &patternSelected;
} else {
pat = view->getPattern(getPen().getLineType());
}
if (pat==NULL) {
return;
}
if (getRadius()<1.0e-6) {
return;
}
// Pen to draw pattern is always solid:
RS_Pen pen = painter->getPen();
pen.setLineType(RS2::SolidLine);
painter->setPen(pen);
double a1;
double a2;
if (data.reversed) {
a2 = getAngle1();
a1 = getAngle2();
} else {
a1 = getAngle1();
a2 = getAngle2();
}
double* da; // array of distances in x.
int i; // index counter
double length = getAngleLength();
// create scaled pattern:
da = new double[pat->num];
for (i=0; i<pat->num; ++i) {
da[i] = fabs(pat->pattern[i] * styleFactor) / getRadius();
}
double tot=0.0;
i=0;
bool done = false;
double curA = a1;
//double cx = getCenter().x * factor.x + offsetX;
//double cy = - a->getCenter().y * factor.y + getHeight() - offsetY;
RS_Vector cp = view->toGui(getCenter());
double r = getRadius() * view->getFactor().x;
do {
if (pat->pattern[i] > 0.0) {
if (tot+da[i]<length) {
painter->drawArc(cp, r,
curA,
curA + da[i],
false);
} else {
painter->drawArc(cp, r,
curA,
a2,
false);
}
}
curA+=da[i];
tot+=da[i];
done=tot>length;
i++;
if (i>=pat->num) {
i=0;
}
} while(!done);
delete[] da;
}
}
bool ompl::geometric::LightningRetrieveRepair::findBestPath(const base::State *startState, const base::State *goalState, ompl::base::PlannerDataPtr &chosenPath)
{
OMPL_INFORM("LightningRetrieveRepair: Found %d similar paths. Filtering", nearestPaths_.size());
// Filter down to just 1 chosen path
ompl::base::PlannerDataPtr bestPath = nearestPaths_.front();
std::size_t bestPathScore = std::numeric_limits<std::size_t>::max();
// Track which path has the shortest distance
std::vector<double> distances(nearestPaths_.size(), 0);
std::vector<bool> isReversed(nearestPaths_.size());
assert(isReversed.size() == nearestPaths_.size());
for (std::size_t pathID = 0; pathID < nearestPaths_.size(); ++pathID)
{
const ompl::base::PlannerDataPtr ¤tPath = nearestPaths_[pathID];
// Error check
if (currentPath->numVertices() < 2) // needs at least a start and a goal
{
OMPL_ERROR("A path was recalled that somehow has less than 2 vertices, which shouldn't happen");
return false;
}
const ompl::base::State *pathStartState = currentPath->getVertex(0).getState();
const ompl::base::State *pathGoalState = currentPath->getVertex(currentPath->numVertices()-1).getState();
double regularDistance = si_->distance(startState,pathStartState) + si_->distance(goalState,pathGoalState);
double reversedDistance = si_->distance(startState,pathGoalState) + si_->distance(goalState,pathStartState);
// Check if path is reversed from normal [start->goal] direction and cache the distance
if ( regularDistance > reversedDistance )
{
// The distance between starts and goals is less when in reverse
isReversed[pathID] = true;
distances[pathID] = reversedDistance;
// We won't actually flip it until later to save memory operations and not alter our NN tree in the LightningDB
}
else
{
isReversed[pathID] = false;
distances[pathID] = regularDistance;
}
std::size_t pathScore = 0; // the score
// Check the validity between our start location and the path's start
// TODO: this might bias the score to be worse for the little connecting segment
if (!isReversed[pathID])
pathScore += checkMotionScore(startState, pathStartState);
else
pathScore += checkMotionScore(startState, pathGoalState);
// Score current path for validity
std::size_t invalidStates = 0;
for (std::size_t vertex_id = 0; vertex_id < currentPath->numVertices(); ++vertex_id)
{
// Check if the sampled points are valid
if (!si_->isValid( currentPath->getVertex(vertex_id).getState()))
{
invalidStates++;
}
}
// Track separate for debugging
pathScore += invalidStates;
// Check the validity between our goal location and the path's goal
// TODO: this might bias the score to be worse for the little connecting segment
if (!isReversed[pathID])
pathScore += checkMotionScore( goalState, pathGoalState );
else
pathScore += checkMotionScore( goalState, pathStartState );
// Factor in the distance between start/goal and our new start/goal
OMPL_INFORM("LightningRetrieveRepair: Path %d | %d verticies | %d invalid | score %d | reversed: %s | distance: %f",
int(pathID), currentPath->numVertices(), invalidStates, pathScore,
isReversed[pathID] ? "true" : "false", distances[pathID]);
// Check if we have a perfect score (0) and this is the shortest path (the first one)
if (pathID == 0 && pathScore == 0)
{
OMPL_DEBUG("LightningRetrieveRepair: --> The shortest path (path 0) has a perfect score (0), ending filtering early.");
bestPathScore = pathScore;
bestPath = currentPath;
nearestPathsChosenID_ = pathID;
break; // end the for loop
}
// Check if this is the best score we've seen so far
if (pathScore < bestPathScore)
{
OMPL_DEBUG("LightningRetrieveRepair: --> This path is the best we've seen so far. Previous best: %d", bestPathScore);
bestPathScore = pathScore;
bestPath = currentPath;
nearestPathsChosenID_ = pathID;
}
// if the best score is the same as a previous one we've seen,
// choose the one that has the shortest connecting component
else if (pathScore == bestPathScore && distances[nearestPathsChosenID_] > distances[pathID])
{
// This new path is a shorter distance
OMPL_DEBUG("LightningRetrieveRepair: --> This path is as good as the best we've seen so far, but its path is shorter. Previous best score: %d from index %d",
bestPathScore, nearestPathsChosenID_);
bestPathScore = pathScore;
bestPath = currentPath;
nearestPathsChosenID_ = pathID;
}
else
OMPL_DEBUG("LightningRetrieveRepair: --> Not best. Best score: %d from index %d", bestPathScore, nearestPathsChosenID_);
}
// Check if we have a solution
if (!bestPath)
{
OMPL_ERROR("LightningRetrieveRepair: No best path found from k filtered paths");
return false;
}
else if(!bestPath->numVertices() || bestPath->numVertices() == 1)
{
OMPL_ERROR("LightningRetrieveRepair: Only %d verticies found in PlannerData loaded from file. This is a bug.", bestPath->numVertices());
return false;
}
// Reverse the path if necessary. We allocate memory for this so that we don't alter the database
if (isReversed[nearestPathsChosenID_])
{
OMPL_DEBUG("LightningRetrieveRepair: Reversing planner data verticies count %d", bestPath->numVertices());
ompl::base::PlannerDataPtr newPath(new ompl::base::PlannerData(si_));
for (std::size_t i = bestPath->numVertices(); i > 0; --i) // size_t can't go negative so subtract 1 instead
{
newPath->addVertex( bestPath->getVertex(i-1) );
}
// Set result
chosenPath = newPath;
}
else
{
// Set result
chosenPath = bestPath;
}
OMPL_DEBUG("LightningRetrieveRepair: Done Filtering\n");
return true;
}
CC3ActionInterval* CC3ActionAnimate::reverse()
{
CC3ActionAnimate* newAnim = actionWithDuration( getDuration() );
newAnim->setIsReversed( !isReversed() );
return newAnim;
}
