void TQuadraticLengthEvaluator::setQuad(const TQuadratic &quad) {
const TPointD &p0 = quad.getP0();
const TPointD &p1 = quad.getP1();
const TPointD &p2 = quad.getP2();
TPointD speed0(2.0 * (p1 - p0));
TPointD accel(2.0 * (p2 - p1) - speed0);
double a = accel * accel;
double b = 2.0 * accel * speed0;
m_c = speed0 * speed0;
m_constantSpeed = isAlmostZero(a); // => b isAlmostZero, too
if (m_constantSpeed) {
m_c = sqrt(m_c);
return;
}
m_sqrt_a_div_2 = 0.5 * sqrt(a);
m_noSpeed0 = isAlmostZero(m_c); // => b isAlmostZero, too
if (m_noSpeed0) return;
m_tRef = 0.5 * b / a;
double d = m_c - 0.5 * b * m_tRef;
m_squareIntegrand = (d < TConsts::epsilon);
if (m_squareIntegrand) {
m_f = (b > 0) ? -sq(m_tRef) : sq(m_tRef);
return;
}
m_e = d / a;
double sqrt_part = sqrt(sq(m_tRef) + m_e);
double log_arg = m_tRef + sqrt_part;
m_squareIntegrand = (log_arg < TConsts::epsilon);
if (m_squareIntegrand) {
m_f = (b > 0) ? -sq(m_tRef) : sq(m_tRef);
return;
}
m_primitive_0 = m_sqrt_a_div_2 * (m_tRef * sqrt_part + m_e * log(log_arg));
}
std::shared_ptr<raytracer::Primitive> Ring::ring(double outerRadius, double innerRadius, double height) const {
if (isAlmostZero(innerRadius)) {
return closedCylinder(outerRadius, height);
} else {
auto result = std::make_shared<raytracer::Difference>();
result->add(closedCylinder(outerRadius, height));
result->add(closedCylinder(innerRadius, height + 0.0001));
return result;
}
}
void detectLanding() {
// if we are not trying to move by setting motor outputs
if (isAlmostZero(goal_.roll()) && isAlmostZero(goal_.pitch()) && isAlmostZero(goal_.yaw()) && isGreaterThanMinThrottle(goal_.throttle()))
{
// and we are not currently moving (based on current velocities)
auto angular = state_estimator_->getAngularVelocity();
auto velocity = state_estimator_->getLinearVelocity();
if (isAlmostZero(angular.roll()) && isAlmostZero(angular.pitch()) && isAlmostZero(angular.yaw()) &&
isAlmostZero(velocity.roll()) && isAlmostZero(velocity.pitch()) && isAlmostZero(velocity.yaw())) {
// then we must be landed...
landed_ = true;
return;
}
}
landed_ = false;
}
void OutlineStrokeProp::draw(const TVectorRenderData &rd) {
if (rd.m_clippingRect != TRect() && !rd.m_is3dView &&
!convert(rd.m_aff * m_stroke->getBBox()).overlaps(rd.m_clippingRect))
return;
glPushMatrix();
tglMultMatrix(rd.m_aff);
double pixelSize = sqrt(tglGetPixelSize2());
#ifdef _DEBUG
if (m_stroke->isCenterLine() && m_colorStyle->getTagId() != 99)
#else
if (m_stroke->isCenterLine())
#endif
{
TCenterLineStrokeStyle *appStyle =
new TCenterLineStrokeStyle(m_colorStyle->getAverageColor(), 0, 0);
appStyle->drawStroke(rd.m_cf, m_stroke);
delete appStyle;
} else {
if (!isAlmostZero(pixelSize - m_outlinePixelSize, 1e-5) ||
m_strokeChanged ||
m_styleVersionNumber != m_colorStyle->getVersionNumber()) {
m_strokeChanged = false;
m_outlinePixelSize = pixelSize;
TOutlineUtil::OutlineParameter param;
m_outline.getArray().clear();
m_colorStyle->computeOutline(m_stroke, m_outline, param);
// TOutlineStyle::StrokeOutlineModifier *modifier =
// m_colorStyle->getStrokeOutlineModifier();
// if(modifier)
// modifier->modify(m_outline);
m_styleVersionNumber = m_colorStyle->getVersionNumber();
}
m_colorStyle->drawStroke(rd.m_cf, &m_outline, m_stroke);
}
glPopMatrix();
}
void VectorBrushProp::draw(const TVectorRenderData &rd)
{
//Ensure that the stroke overlaps our clipping rect
if (rd.m_clippingRect != TRect() && !rd.m_is3dView &&
!convert(rd.m_aff * m_stroke->getBBox()).overlaps(rd.m_clippingRect))
return;
TPaletteP palette(m_brush->getPalette());
if (!palette)
return;
static TOutlineUtil::OutlineParameter param; //unused, but requested
//Build a solid color style to draw each m_vi's stroke with.
TSolidColorStyle colorStyle;
//Push the specified rd affine before drawing
glPushMatrix();
tglMultMatrix(rd.m_aff);
//1. If necessary, build the outlines
double currentPixelSize = sqrt(tglGetPixelSize2());
bool differentPixelSize = !isAlmostZero(currentPixelSize - m_pixelSize, 1e-5);
m_pixelSize = currentPixelSize;
int i, viRegionsCount = m_brush->getRegionCount(), viStrokesCount = m_brush->getStrokeCount();
if (differentPixelSize || m_strokeChanged) {
m_strokeChanged = false;
//1a. First, the regions
m_regionOutlines.resize(viRegionsCount);
for (i = 0; i < viRegionsCount; ++i) {
TRegionOutline &outline = m_regionOutlines[i];
const TRegion *brushRegion = m_brush->getRegion(i);
//Build the outline
outline.clear();
TOutlineUtil::makeOutline(*getStroke(), *brushRegion, m_brushBox,
outline);
}
//1b. Then, the strokes
m_strokeOutlines.resize(viStrokesCount);
for (i = 0; i < viStrokesCount; ++i) {
TStrokeOutline &outline = m_strokeOutlines[i];
const TStroke *brushStroke = m_brush->getStroke(i);
outline.getArray().clear();
TOutlineUtil::makeOutline(*getStroke(), *brushStroke, m_brushBox,
outline, param);
}
}
//2. Draw the outlines
UINT s, t, r,
strokesCount = m_brush->getStrokeCount(),
regionCount = m_brush->getRegionCount();
for (s = 0; s < strokesCount; s = t) //Each cycle draws a group
{
//A vector image stores group strokes with consecutive indices.
//2a. First, draw regions in the strokeIdx-th stroke's group
for (r = 0; r < regionCount; ++r) {
if (m_brush->sameGroupStrokeAndRegion(s, r)) {
const TRegion *brushRegion = m_brush->getRegion(r);
const TColorStyle *brushStyle =
palette->getStyle(brushRegion->getStyle());
assert(brushStyle);
//Draw the outline
colorStyle.setMainColor(brushStyle->getMainColor());
colorStyle.drawRegion(0, false, m_regionOutlines[r]);
}
}
//2b. Then, draw all strokes in strokeIdx-th stroke's group
for (t = s; t < strokesCount && m_brush->sameGroup(s, t); ++t) {
const TStroke *brushStroke = m_brush->getStroke(t);
const TColorStyle *brushStyle =
palette->getStyle(brushStroke->getStyle());
if (!brushStyle)
continue;
colorStyle.setMainColor(brushStyle->getMainColor());
colorStyle.drawStroke(0, &m_strokeOutlines[t], brushStroke); //brushStroke unused but requested
}
}
glPopMatrix();
}
