// All-in-one (blocking): Returns temperature, humidity, & dewpoint
uint8_t Sensirion::measure(float *temp, float *humi, float *dew) {
uint16_t rawData;
uint8_t error;
if (error = meas(TEMP, &rawData, BLOCK))
return error;
*temp = calcTemp(rawData);
if (error = meas(HUMI, &rawData, BLOCK))
return error;
*humi = calcHumi(rawData, *temp);
*dew = calcDewpoint(*humi, *temp);
return 0 ;
}
bool device_gyro::calibrate()
{
static const uint_t n = 512;
std::vector<vec3d> meas(n);
// measure
for (uint_t i = 0; i < n; ++ i)
{
meas[i] = angular_velocity();
sysclock.delay(1);
}
// mean value
vec3d mean;
for (uint_t i = 0; i < n; ++ i)
mean = mean + meas[i];
mean = mean / n;
// standard deviation
float dev = 0;
for (uint_t i = 0; i < n; ++ i)
dev += (meas[i] - mean).square();
dev = sqrt(dev / n);
if (dev > 0.05)
return false;
w0 = w0 + mean;
return true;
}
transform3d<T> transformation3(IT begin, IT end, bool& success)
{
minimath::matrix<T,4,3> ref; // 4x3 to allow for rotation + translation
minimath::matrix<T,3> meas;
unsigned int index = 0;
// to-do write something to assign elements to matrix
for (IT iPair = begin; iPair !=end; ++iPair)
{
for (unsigned int i = 0; i < 3; ++i)
{
ref(i, index) = (*iPair)[0][i];
meas(i, index) = (*iPair)[1][i];
}
++index;
}
minimath::setRow(ref, point3d<T>(1,1,1), 3);
// find rotation!
matrix<T, 3, 4> rot = transformation(ref, meas, success);
return success ? transform3d<T>(rot) : transform3d<T>();
}
static void test_small_segment(skiatest::Reporter* reporter) {
#ifdef SK_SCALAR_IS_FLOAT
SkPath path;
const SkPoint pts[] = {
{ 100000, 100000},
// big jump between these points, makes a big segment
{ SkFloatToScalar(1.0005f), SkFloatToScalar(0.9999f) },
// tiny (non-zero) jump between these points
{ SK_Scalar1, SK_Scalar1 },
};
path.moveTo(pts[0]);
for (size_t i = 1; i < SK_ARRAY_COUNT(pts); ++i) {
path.lineTo(pts[i]);
}
SkPathMeasure meas(path, false);
/* this would assert (before a fix) because we added a segment with
the same length as the prev segment, due to the follow (bad) pattern
d = distance(pts[0], pts[1]);
distance += d;
seg->fDistance = distance;
SkASSERT(d > 0); // TRUE
SkASSERT(seg->fDistance > prevSeg->fDistance); // FALSE
This 2nd assert failes because (distance += d) didn't affect distance
because distance >>> d.
*/
meas.getLength();
#endif
}
void PressureSensorHandler::MeasurementCallback(
const geometry_msgs::PointStampedConstPtr & msg) {
received_first_measurement_ = true;
this->SequenceWatchDog(msg->header.seq, subPressure_.getTopic());
MSF_INFO_STREAM_ONCE(
"*** pressure sensor got first measurement from topic "
<< this->topic_namespace_ << "/" << subPressure_.getTopic()
<< " ***");
bool throttle = true;
if (throttle && msg->header.seq % 10 != 0) {
return;
}
shared_ptr<pressure_measurement::PressureMeasurement> meas(
new pressure_measurement::PressureMeasurement(
n_zp_, true, this->sensorID, enable_mah_outlier_rejection_,
mah_threshold_));
meas->MakeFromSensorReading(msg, msg->header.stamp.toSec());
z_p_ = meas->z_p_; // Store this for the init procedure.
// Make averaged measurement.
memcpy(heightbuff, heightbuff + 1, sizeof(double) * (heightbuffsize - 1));
heightbuff[heightbuffsize - 1] = meas->z_p_(0);
double sum = 0;
for (int k = 0; k < heightbuffsize; ++k)
sum += heightbuff[k];
z_average_p(0) = sum / heightbuffsize;
this->manager_.msf_core_->AddMeasurement(meas);
}
bool SkTrimPE::filterPath(SkPath* dst, const SkPath& src, SkStrokeRec* rec,
const SkRect* cullRect) const {
if (fStartT >= fStopT) {
SkASSERT(fMode == SkTrimPathEffect::Mode::kNormal);
return true;
}
// First pass: compute the total len.
SkScalar len = 0;
SkPathMeasure meas(src, false);
do {
len += meas.getLength();
} while (meas.nextContour());
const auto arcStart = len * fStartT,
arcStop = len * fStopT;
// Second pass: actually add segments.
Segmentator segmentator(src, dst);
if (fMode == SkTrimPathEffect::Mode::kNormal) {
if (arcStart < arcStop) segmentator.add(arcStart, arcStop);
} else {
if (0 < arcStart) segmentator.add(0, arcStart);
if (arcStop < len) segmentator.add(arcStop, len);
}
return true;
}
// Regression test for b/26425223
DEF_TEST(PathMeasure_nextctr, reporter) {
SkPath path;
path.moveTo(0, 0); path.lineTo(100, 0);
SkPathMeasure meas(path, false);
// only expect 1 contour, even if we didn't explicitly call getLength() ourselves
REPORTER_ASSERT(reporter, !meas.nextContour());
}
// read trk format
BOOLINT minipath::read_trk_format(ministrings &trk)
{
unsigned int i,j;
if (trk.empty()) return(FALSE);
if (trk[0]=="[track]")
for (i=1; i<trk.getsize(); i++)
if (trk[i]=="--start--")
{
for (j=i+1; j<trk.getsize(); j++)
{
ministring line=trk[j];
if (line.startswith("("))
{
double lat,lon,elev,time;
line=line.tail("(");
lat=line.prefix(",").value();
line=line.tail(",");
lon=line.prefix(",").value();
line=line.tail(",");
elev=line.prefix(",").value();
line=line.tail(",");
time=line.prefix(",").value();
line=line.tail(")");
minicoord coord(miniv4d(lon*3600,lat*3600,elev,time),
minicoord::MINICOORD_LLH,0,minicoord::MINICOORD_DATUM_NONE,crs_orb);
BOOLINT start=FALSE;
ministring desc;
if (line.startswith(";"))
{
line=line.tail(";");
if (line=="#CMDNewSegment") start=TRUE;
else desc=line;
line.clear();
}
minimeas meas(coord,NAN,NAN,NAN,start);
meas.set_description(desc);
append(meas);
}
if (!line.empty()) return(FALSE);
}
return(TRUE);
}
return(FALSE);
}
bool Sk1DPathEffect::filterPath(SkPath* dst, const SkPath& src, SkScalar* width) {
SkPathMeasure meas(src, false);
do {
SkScalar length = meas.getLength();
SkScalar distance = this->begin(length);
while (distance < length) {
SkScalar delta = this->next(dst, distance, meas);
if (delta <= 0) {
break;
}
distance += delta;
}
} while (meas.nextContour());
return true;
}
static void test_small_segment2() {
SkPath path;
const SkPoint pts[] = {
{ 0, 0 },
{ 100000000000.0f, 100000000000.0f }, { 0, 0 },
{ 10, 10 }, { 0, 0 },
};
path.moveTo(pts[0]);
for (size_t i = 1; i < SK_ARRAY_COUNT(pts); i += 2) {
path.quadTo(pts[i], pts[i + 1]);
}
SkPathMeasure meas(path, false);
meas.getLength();
}
static void test_small_segment2(skiatest::Reporter* reporter) {
#ifdef SK_SCALAR_IS_FLOAT
SkPath path;
const SkPoint pts[] = {
{ 0, 0 },
{ 100000000000.0f, 100000000000.0f }, { 0, 0 },
{ 10, 10 }, { 0, 0 },
};
path.moveTo(pts[0]);
for (size_t i = 1; i < SK_ARRAY_COUNT(pts); i += 2) {
path.quadTo(pts[i], pts[i + 1]);
}
SkPathMeasure meas(path, false);
meas.getLength();
#endif
}
void SkBaseDevice::drawTextOnPath(const SkDraw& draw, const void* text, size_t byteLength,
const SkPath& follow, const SkMatrix* matrix,
const SkPaint& paint) {
SkASSERT(byteLength == 0 || text != nullptr);
// nothing to draw
if (text == nullptr || byteLength == 0 || draw.fRC->isEmpty()) {
return;
}
SkTextToPathIter iter((const char*)text, byteLength, paint, true);
SkPathMeasure meas(follow, false);
SkScalar hOffset = 0;
// need to measure first
if (paint.getTextAlign() != SkPaint::kLeft_Align) {
SkScalar pathLen = meas.getLength();
if (paint.getTextAlign() == SkPaint::kCenter_Align) {
pathLen = SkScalarHalf(pathLen);
}
hOffset += pathLen;
}
const SkPath* iterPath;
SkScalar xpos;
SkMatrix scaledMatrix;
SkScalar scale = iter.getPathScale();
scaledMatrix.setScale(scale, scale);
while (iter.next(&iterPath, &xpos)) {
if (iterPath) {
SkPath tmp;
SkMatrix m(scaledMatrix);
tmp.setIsVolatile(true);
m.postTranslate(xpos + hOffset, 0);
if (matrix) {
m.postConcat(*matrix);
}
morphpath(&tmp, *iterPath, meas, m);
this->drawPath(draw, tmp, iter.getPaint(), nullptr, true);
}
}
}
private: void process_control(des_event_type const& evt, des_engine_context_type& ctx)
{
::std::vector<real_type> measures;
::std::vector<real_type> ref_measures;
//measures = ptr_app_->statistic(response_time_application_statistic)->retrieve();
measures = ptr_app_->performance_measures()->retrieve();
ref_measures = ptr_app_->reference_performance_measures();
vector_type meas(
measures.begin(),
measures.end()
);
vector_type ref_meas(
ref_measures.begin(),
ref_measures.end()
);
vector_type shares = controller_.control(meas-ref_meas);
}
void draw_ribs(SkCanvas* canvas, const SkPath& path, SkScalar width,
SkColor color) {
const SkScalar radius = width / 2;
SkPathMeasure meas(path, false);
SkScalar total = meas.getLength();
SkScalar delta = 8;
SkPaint paint;
paint.setColor(color);
SkPoint pos, tan;
for (SkScalar dist = 0; dist <= total; dist += delta) {
if (meas.getPosTan(dist, &pos, &tan)) {
tan.scale(radius);
tan.rotateCCW();
canvas->drawLine(pos.x() + tan.x(), pos.y() + tan.y(),
pos.x() - tan.x(), pos.y() - tan.y(), paint);
}
}
}
static void textPathMatrix(SkCanvas* canvas) {
SkPaint paint;
SkPath path;
SkMatrix matrix;
path.moveTo(SkIntToScalar(050), SkIntToScalar(200));
path.quadTo(SkIntToScalar(250), SkIntToScalar(000),
SkIntToScalar(450), SkIntToScalar(200));
paint.setAntiAlias(true);
paint.setStyle(SkPaint::kStroke_Style);
canvas->drawPath(path, paint);
paint.setStyle(SkPaint::kFill_Style);
paint.setTextSize(SkIntToScalar(48));
paint.setTextAlign(SkPaint::kRight_Align);
const char* text = "Reflection";
size_t len = strlen(text);
SkPathMeasure meas(path, false);
SkScalar pathLen = meas.getLength();
canvas->drawTextOnPath(text, len, path, NULL, paint);
paint.setColor(SK_ColorRED);
matrix.setScale(-SK_Scalar1, SK_Scalar1);
matrix.postTranslate(pathLen, 0);
canvas->drawTextOnPath(text, len, path, &matrix, paint);
paint.setColor(SK_ColorBLUE);
matrix.setScale(SK_Scalar1, -SK_Scalar1);
canvas->drawTextOnPath(text, len, path, &matrix, paint);
paint.setColor(SK_ColorGREEN);
matrix.setScale(-SK_Scalar1, -SK_Scalar1);
matrix.postTranslate(pathLen, 0);
canvas->drawTextOnPath(text, len, path, &matrix, paint);
}
static void TestPathMeasure(skiatest::Reporter* reporter) {
SkPath path;
path.moveTo(0, 0);
path.lineTo(SK_Scalar1, 0);
path.lineTo(SK_Scalar1, SK_Scalar1);
path.lineTo(0, SK_Scalar1);
SkPathMeasure meas(path, true);
SkScalar length = meas.getLength();
SkASSERT(length == SK_Scalar1*4);
path.reset();
path.moveTo(0, 0);
path.lineTo(SK_Scalar1*3, SK_Scalar1*4);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1*5);
path.reset();
path.addCircle(0, 0, SK_Scalar1);
meas.setPath(&path, true);
length = meas.getLength();
// SkDebugf("circle arc-length = %g\n", length);
for (int i = 0; i < 8; i++) {
SkScalar d = length * i / 8;
SkPoint p;
SkVector v;
meas.getPosTan(d, &p, &v);
#if 0
SkDebugf("circle arc-length=%g, pos[%g %g] tan[%g %g]\n",
d, p.fX, p.fY, v.fX, v.fY);
#endif
}
// Test the behavior following a close not followed by a move.
path.reset();
path.lineTo(SK_Scalar1, 0);
path.lineTo(SK_Scalar1, SK_Scalar1);
path.lineTo(0, SK_Scalar1);
path.close();
path.lineTo(-SK_Scalar1, 0);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1 * 4);
meas.nextContour();
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1);
SkPoint position;
SkVector tangent;
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, -SK_ScalarHalf, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter, position.fY == 0);
REPORTER_ASSERT(reporter, tangent.fX == -SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
// Test degenerate paths
path.reset();
path.moveTo(0, 0);
path.lineTo(0, 0);
path.lineTo(SK_Scalar1, 0);
path.quadTo(SK_Scalar1, 0, SK_Scalar1, 0);
path.quadTo(SK_Scalar1, SK_Scalar1, SK_Scalar1, SK_Scalar1 * 2);
path.cubicTo(SK_Scalar1, SK_Scalar1 * 2,
SK_Scalar1, SK_Scalar1 * 2,
SK_Scalar1, SK_Scalar1 * 2);
path.cubicTo(SK_Scalar1*2, SK_Scalar1 * 2,
SK_Scalar1*3, SK_Scalar1 * 2,
SK_Scalar1*4, SK_Scalar1 * 2);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1 * 6);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, SK_ScalarHalf, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter, position.fY == 0);
REPORTER_ASSERT(reporter, tangent.fX == SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_Scalar1 * 2.5f, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, SK_Scalar1, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fY, SK_Scalar1 * 1.5f));
REPORTER_ASSERT(reporter, tangent.fX == 0);
REPORTER_ASSERT(reporter, tangent.fY == SK_Scalar1);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_Scalar1 * 4.5f, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, SK_Scalar1 * 2.5f, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fY, SK_Scalar1 * 2.0f, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter, tangent.fX == SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
path.reset();
path.moveTo(0, 0);
path.lineTo(SK_Scalar1, 0);
path.moveTo(SK_Scalar1, SK_Scalar1);
path.moveTo(SK_Scalar1 * 2, SK_Scalar1 * 2);
path.lineTo(SK_Scalar1, SK_Scalar1 * 2);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, SK_ScalarHalf, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter, position.fY == 0);
REPORTER_ASSERT(reporter, tangent.fX == SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
meas.nextContour();
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, SK_Scalar1 * 1.5f, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fY, SK_Scalar1 * 2.0f, SK_Scalar1 * 0.0001));
REPORTER_ASSERT(reporter, tangent.fX == -SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
}
static void TestPathMeasure(skiatest::Reporter* reporter) {
SkPath path;
path.moveTo(0, 0);
path.lineTo(SK_Scalar1, 0);
path.lineTo(SK_Scalar1, SK_Scalar1);
path.lineTo(0, SK_Scalar1);
SkPathMeasure meas(path, true);
SkScalar length = meas.getLength();
SkASSERT(length == SK_Scalar1*4);
path.reset();
path.moveTo(0, 0);
path.lineTo(SK_Scalar1*3, SK_Scalar1*4);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1*5);
path.reset();
path.addCircle(0, 0, SK_Scalar1);
meas.setPath(&path, true);
length = meas.getLength();
// SkDebugf("circle arc-length = %g\n", length);
// Test the behavior following a close not followed by a move.
path.reset();
path.lineTo(SK_Scalar1, 0);
path.lineTo(SK_Scalar1, SK_Scalar1);
path.lineTo(0, SK_Scalar1);
path.close();
path.lineTo(-SK_Scalar1, 0);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1 * 4);
meas.nextContour();
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1);
SkPoint position;
SkVector tangent;
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX,
-SK_ScalarHalf,
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter, position.fY == 0);
REPORTER_ASSERT(reporter, tangent.fX == -SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
// Test degenerate paths
path.reset();
path.moveTo(0, 0);
path.lineTo(0, 0);
path.lineTo(SK_Scalar1, 0);
path.quadTo(SK_Scalar1, 0, SK_Scalar1, 0);
path.quadTo(SK_Scalar1, SK_Scalar1, SK_Scalar1, SK_Scalar1 * 2);
path.cubicTo(SK_Scalar1, SK_Scalar1 * 2,
SK_Scalar1, SK_Scalar1 * 2,
SK_Scalar1, SK_Scalar1 * 2);
path.cubicTo(SK_Scalar1*2, SK_Scalar1 * 2,
SK_Scalar1*3, SK_Scalar1 * 2,
SK_Scalar1*4, SK_Scalar1 * 2);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1 * 6);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX,
SK_ScalarHalf,
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter, position.fY == 0);
REPORTER_ASSERT(reporter, tangent.fX == SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
REPORTER_ASSERT(reporter, meas.getPosTan(SkFloatToScalar(2.5f), &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX, SK_Scalar1, SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fY, SkFloatToScalar(1.5f)));
REPORTER_ASSERT(reporter, tangent.fX == 0);
REPORTER_ASSERT(reporter, tangent.fY == SK_Scalar1);
REPORTER_ASSERT(reporter, meas.getPosTan(SkFloatToScalar(4.5f), &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX,
SkFloatToScalar(2.5f),
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fY,
SkFloatToScalar(2.0f),
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter, tangent.fX == SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
path.reset();
path.moveTo(0, 0);
path.lineTo(SK_Scalar1, 0);
path.moveTo(SK_Scalar1, SK_Scalar1);
path.moveTo(SK_Scalar1 * 2, SK_Scalar1 * 2);
path.lineTo(SK_Scalar1, SK_Scalar1 * 2);
meas.setPath(&path, false);
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX,
SK_ScalarHalf,
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter, position.fY == 0);
REPORTER_ASSERT(reporter, tangent.fX == SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
meas.nextContour();
length = meas.getLength();
REPORTER_ASSERT(reporter, length == SK_Scalar1);
REPORTER_ASSERT(reporter, meas.getPosTan(SK_ScalarHalf, &position, &tangent));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fX,
SkFloatToScalar(1.5f),
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter,
SkScalarNearlyEqual(position.fY,
SkFloatToScalar(2.0f),
SkFloatToScalar(0.0001f)));
REPORTER_ASSERT(reporter, tangent.fX == -SK_Scalar1);
REPORTER_ASSERT(reporter, tangent.fY == 0);
test_small_segment(reporter);
test_small_segment2(reporter);
test_small_segment3(reporter);
}
returnValue SIMexport::exportAndRun( const String& dirName,
const String& initStates,
const String& controls,
const String& results,
const String& ref
)
{
set( GENERATE_TEST_FILE, 1 );
Grid integrationGrid;
modelData.getIntegrationGrid(integrationGrid);
std::vector<Grid> outputGrids;
modelData.getOutputGrids(outputGrids);
int measGrid;
get( MEASUREMENT_GRID, measGrid );
if( (MeasurementGrid)measGrid == ONLINE_GRID || ((MeasurementGrid)measGrid == EQUIDISTANT_SUBGRID && !modelData.hasEquidistantIntegrationGrid()) ) return ACADOERROR( RET_INVALID_OPTION );
_initStates = initStates;
_controls = controls;
_results = results;
_ref = ref;
uint i, j;
Vector meas( (uint)outputGrids.size() );
Vector measRef( (uint)outputGrids.size() );
for( i = 0; i < outputGrids.size(); i++ ) {
meas(i) = (double)outputGrids[i].getNumIntervals();
measRef(i) = (double)outputGrids[i].getNumIntervals()*factorRef;
}
Vector intGrid( integrationGrid.getNumIntervals()+1 );
Vector refIntGrid( factorRef*integrationGrid.getNumIntervals()+1 );
if( !modelData.hasEquidistantIntegrationGrid() ) {
intGrid(0) = integrationGrid.getTime( 0 );
refIntGrid(0) = integrationGrid.getTime( 0 );
for( i = 0; i < integrationGrid.getNumIntervals(); i++ ) {
intGrid(i+1) = integrationGrid.getTime( i+1 );
double step = (integrationGrid.getTime( i+1 ) - integrationGrid.getTime( i ))/factorRef;
for( j = 0; j < factorRef; j++ ) {
refIntGrid(i*factorRef+1+j) = refIntGrid(i*factorRef+j) + step;
}
}
}
int numSteps;
get( NUM_INTEGRATOR_STEPS, numSteps );
timingCalls = (uint) ceil((double)(timingSteps*modelData.getN())/((double) numSteps) - 10.0*EPS);
timingSteps = (uint) ceil((double)timingCalls*((double) numSteps/((double) modelData.getN())) - 10.0*EPS);
if( !referenceProvided ) {
// REFERENCE:
if( !modelData.hasEquidistantIntegrationGrid() ) {
modelData.setMeasurements( meas ); // EQUIDISTANT_GRID option is used
modelData.setIntegrationGrid( refIntGrid );
exportCode( dirName );
exportTest( dirName, String( "test.c" ), _ref, _refOutputFiles, BT_FALSE, 1 );
}
else if( (MeasurementGrid)measGrid == EQUIDISTANT_GRID ) {
modelData.setMeasurements( meas );
set( NUM_INTEGRATOR_STEPS, (int)factorRef*numSteps );
exportCode( dirName );
exportTest( dirName, String( "test.c" ), _ref, _refOutputFiles, BT_FALSE, 1 );
}
else {
modelData.setMeasurements( measRef );
set( NUM_INTEGRATOR_STEPS, (int)factorRef*numSteps );
exportCode( dirName );
exportTest( dirName, String( "test.c" ), _ref, _refOutputFiles, BT_FALSE, factorRef );
}
executeTest( dirName );
}
modelData.clearIntegrationGrid();
// THE INTEGRATOR:
modelData.setMeasurements( meas );
set( NUM_INTEGRATOR_STEPS, numSteps );
if( !modelData.hasEquidistantIntegrationGrid() ) {
modelData.setIntegrationGrid( intGrid );
}
exportCode( dirName );
if(timingSteps > 0 && timingCalls > 0) exportTest( dirName, String( "test.c" ), _results, _outputFiles, BT_TRUE, 1 );
else exportTest( dirName, String( "test.c" ), _results, _outputFiles, BT_FALSE, 1 );
executeTest( dirName );
// THE EVALUATION:
int nil;
nil = system( (String(dirName) << "/./compare").getName() );
return SUCCESSFUL_RETURN;
}
bool SkDashPathEffect::filterPath(SkPath* dst, const SkPath& src,
SkStrokeRec* rec, const SkRect* cullRect) const {
// we do nothing if the src wants to be filled, or if our dashlength is 0
if (rec->isFillStyle() || fInitialDashLength < 0) {
return false;
}
const SkScalar* intervals = fIntervals;
SkScalar dashCount = 0;
int segCount = 0;
SkPath cullPathStorage;
const SkPath* srcPtr = &src;
if (cull_path(src, *rec, cullRect, fIntervalLength, &cullPathStorage)) {
srcPtr = &cullPathStorage;
}
SpecialLineRec lineRec;
bool specialLine = lineRec.init(*srcPtr, dst, rec, fCount >> 1, fIntervalLength);
SkPathMeasure meas(*srcPtr, false);
do {
bool skipFirstSegment = meas.isClosed();
bool addedSegment = false;
SkScalar length = meas.getLength();
int index = fInitialDashIndex;
SkScalar scale = SK_Scalar1;
// Since the path length / dash length ratio may be arbitrarily large, we can exert
// significant memory pressure while attempting to build the filtered path. To avoid this,
// we simply give up dashing beyond a certain threshold.
//
// The original bug report (http://crbug.com/165432) is based on a path yielding more than
// 90 million dash segments and crashing the memory allocator. A limit of 1 million
// segments seems reasonable: at 2 verbs per segment * 9 bytes per verb, this caps the
// maximum dash memory overhead at roughly 17MB per path.
static const SkScalar kMaxDashCount = 1000000;
dashCount += length * (fCount >> 1) / fIntervalLength;
if (dashCount > kMaxDashCount) {
dst->reset();
return false;
}
if (fScaleToFit) {
if (fIntervalLength >= length) {
scale = SkScalarDiv(length, fIntervalLength);
} else {
SkScalar div = SkScalarDiv(length, fIntervalLength);
int n = SkScalarFloorToInt(div);
scale = SkScalarDiv(length, n * fIntervalLength);
}
}
// Using double precision to avoid looping indefinitely due to single precision rounding
// (for extreme path_length/dash_length ratios). See test_infinite_dash() unittest.
double distance = 0;
double dlen = SkScalarMul(fInitialDashLength, scale);
while (distance < length) {
SkASSERT(dlen >= 0);
addedSegment = false;
if (is_even(index) && dlen > 0 && !skipFirstSegment) {
addedSegment = true;
++segCount;
if (specialLine) {
lineRec.addSegment(SkDoubleToScalar(distance),
SkDoubleToScalar(distance + dlen),
dst);
} else {
meas.getSegment(SkDoubleToScalar(distance),
SkDoubleToScalar(distance + dlen),
dst, true);
}
}
distance += dlen;
// clear this so we only respect it the first time around
skipFirstSegment = false;
// wrap around our intervals array if necessary
index += 1;
SkASSERT(index <= fCount);
if (index == fCount) {
index = 0;
}
// fetch our next dlen
dlen = SkScalarMul(intervals[index], scale);
}
// extend if we ended on a segment and we need to join up with the (skipped) initial segment
if (meas.isClosed() && is_even(fInitialDashIndex) &&
fInitialDashLength > 0) {
meas.getSegment(0, SkScalarMul(fInitialDashLength, scale), dst, !addedSegment);
++segCount;
}
} while (meas.nextContour());
if (segCount > 1) {
dst->setConvexity(SkPath::kConcave_Convexity);
}
return true;
}
void PartialVolumeAnalysisClusteringCalculator::InternalGenerateProbabilityImage(
const itk::Image< TPixel, VImageDimension > *image,
const HelperStructClusteringResults clusterResults,
mitk::Image::Pointer outImage1, mitk::Image::Pointer outImage2 ) const
{
typedef itk::Image< TPixel, VImageDimension > ImageType;
typedef itk::Image< itk::RGBAPixel<unsigned char>, VImageDimension > DisplayImageType;
typedef itk::Image< float, VImageDimension > ProbImageType;
typename ProbImageType::Pointer probimage = ProbImageType::New();
probimage->SetSpacing( image->GetSpacing() ); // Set the image spacing
probimage->SetOrigin( image->GetOrigin() ); // Set the image origin
probimage->SetDirection( image->GetDirection() ); // Set the image direction
probimage->SetRegions( image->GetLargestPossibleRegion() );
probimage->Allocate();
probimage->FillBuffer(0);
typename DisplayImageType::Pointer displayimage = DisplayImageType::New();
displayimage->SetSpacing( image->GetSpacing() ); // Set the image spacing
displayimage->SetOrigin( image->GetOrigin() ); // Set the image origin
displayimage->SetDirection( image->GetDirection() ); // Set the image direction
displayimage->SetRegions( image->GetLargestPossibleRegion() );
displayimage->Allocate();
typename DisplayImageType::PixelType rgba;
rgba.Set(0.0f, 0.0f, 0.0f, 0.0f);
displayimage->FillBuffer(rgba);
itk::ImageRegionConstIterator<ImageType>
itimage(image, image->GetLargestPossibleRegion());
itk::ImageRegionIterator<ProbImageType>
itprob(probimage, probimage->GetLargestPossibleRegion());
itk::ImageRegionIterator<DisplayImageType>
itdisp(displayimage, displayimage->GetLargestPossibleRegion());
itimage.GoToBegin();
itprob.GoToBegin();
MitkHistType::IndexType index(1);
float maxp = 0;
while( !itimage.IsAtEnd() )
{
if(itimage.Get())
{
MitkHistType::MeasurementVectorType meas(1);
meas.Fill(itimage.Get());
double aposteriori = 0;
bool success = clusterResults.interestingHist->GetIndex(meas, index );
if(success)
{
double aprioriProb = clusterResults.interestingHist->GetFrequency(index);
double intensityProb = clusterResults.totalHist->GetFrequency(index);
double p_interesting = clusterResults.p_interesting;
aposteriori = p_interesting * aprioriProb / intensityProb;
}
else
{
MITK_ERROR << "index not found in histogram";
}
if(aposteriori > 0.0000000000000001)
{
itprob.Set( aposteriori );
maxp = aposteriori > maxp ? aposteriori : maxp;
}
else
{
itprob.Set(0.0f);
}
}
++itimage;
++itprob;
}
itprob.GoToBegin();
itdisp.GoToBegin();
while( !itprob.IsAtEnd() )
{
if(itprob.Get())
{
typename DisplayImageType::PixelType rgba;
rgba.Set(255.0f, 0.0f, 0.0f, 255.0f*(itprob.Get()/maxp));
itdisp.Set( rgba );
}
++itprob;
++itdisp;
}
outImage1->InitializeByItk(probimage.GetPointer());
outImage1->SetVolume(probimage->GetBufferPointer());
outImage2->InitializeByItk(displayimage.GetPointer());
outImage2->SetVolume(displayimage->GetBufferPointer());
}
bool SkDashPathEffect::filterPath(SkPath* dst, const SkPath& src, SkScalar* width)
{
// we do nothing if the src wants to be filled, or if our dashlength is 0
if (*width < 0 || fInitialDashLength < 0)
return false;
SkPathMeasure meas(src, false);
const SkScalar* intervals = fIntervals;
do {
bool skipFirstSegment = meas.isClosed();
bool addedSegment = false;
SkScalar length = meas.getLength();
int index = fInitialDashIndex;
SkScalar scale = SK_Scalar1;
if (fScaleToFit)
{
if (fIntervalLength >= length)
scale = SkScalarDiv(length, fIntervalLength);
else
{
SkScalar div = SkScalarDiv(length, fIntervalLength);
int n = SkScalarFloor(div);
scale = SkScalarDiv(length, n * fIntervalLength);
}
}
SkScalar distance = 0;
SkScalar dlen = SkScalarMul(fInitialDashLength, scale);
while (distance < length)
{
SkASSERT(dlen >= 0);
addedSegment = false;
if (is_even(index) && dlen > 0 && !skipFirstSegment)
{
addedSegment = true;
meas.getSegment(distance, distance + dlen, dst, true);
}
distance += dlen;
// clear this so we only respect it the first time around
skipFirstSegment = false;
// wrap around our intervals array if necessary
index += 1;
SkASSERT(index <= fCount);
if (index == fCount)
index = 0;
// fetch our next dlen
dlen = SkScalarMul(intervals[index], scale);
}
// extend if we ended on a segment and we need to join up with the (skipped) initial segment
if (meas.isClosed() && is_even(fInitialDashIndex) && fInitialDashLength > 0)
meas.getSegment(0, SkScalarMul(fInitialDashLength, scale), dst, !addedSegment);
} while (meas.nextContour());
return true;
}
static SkScalar getpathlen(const SkPath& path) {
SkPathMeasure meas(path, false);
return meas.getLength();
}
void KalmanModel::Update(float featureX,float featureY,float distanceVal,float bearingVal,float distanceDev,float bearingDev){
float q;
matrix6_6 identityM;
float observationDistance;
float observationBearing;
matrix2_2 st;
matrix6_2 kt;
matrix2_1 obs;
matrix2_6 ht;
matrix2_2 qt,stInv;
matrix2_1 meas;
matrix2_1 diff;
stInv.zero();
// Observation variance
qt.zero();
qt(0,0) = distanceDev * distanceDev;
qt(1,1) = bearingDev * bearingDev;
//qt.prettyPrint();
// Observation distance and bearing
obs.zero();
obs(0,0) = distanceVal;
obs(1,0) = bearingVal;
//obs.prettyPrint();
diff.zero();
st.zero();
kt.zero();
meas.zero();
ht.zero();
q = pow( state(0,0) - featureX, 2 ) + pow( ( state(1,0) - featureY ), 2);
observationDistance = sqrt(q);
//cout << "expected distance" << observationDistance << endl;
observationBearing = anglediff2(atan2(featureY - state(1,0) ,featureX - state(0,0)),state(2,0));
//cout << "expected bearing" << observationBearing << endl;
meas(0,0) = observationDistance;
meas(1,0) = observationBearing;
//meas.prettyPrint();
ht(0,0) = - ( featureX - state(0,0) ) / sqrt(q);
ht(0,1) = - ( featureY - state(1,0) ) / sqrt(q);
ht(0,2) = 0;
ht(0,3) = 0;
ht(0,4) = 0;
ht(0,5) = 0;
ht(1,0) = ( featureY - state(1,0) ) / q;
ht(1,1) = - ( featureX - state(0,0) ) / q;
ht(1,2) = -1;
ht(1,3) = 0;
ht(1,4) = 0;
ht(1,5) = 0;
//ht.prettyPrint();
st = ((ht.slow_mult(var)).slow_mult(ht.transp())).add(qt);
//st.prettyPrint();
stInv = st;
invert_square_matrix(stInv);
diff(0,0) = obs(0,0) - meas(0,0);
diff(1,0) = anglediff2(obs(1,0),meas(1,0));
//diff.prettyPrint();
kt = (var.slow_mult(ht.transp())).slow_mult(stInv);
state = state.add(kt.slow_mult(diff));
var = ((identityM.identity().sub(kt.slow_mult(ht)))).slow_mult(var);
double w = 1.0/(2*M_PI) * 1/sqrt((st(0,0) * st(1,1)-st(0,1)*st(1,0))) * exp(-1.0/2 *(diff.transp()).slow_mult(stInv).slow_mult(diff));
mWeight *= w;
}
