<commit_before>// Copyright 2014 Google Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * DSP functions and operations.
 */

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstring>
#include <memory>
#include <stdint.h>
#include <vector>

#include "dsp.h"

using namespace std;

namespace radioreceiver {

const double kPi = 3.141592653989793238;
const double k2Pi = 2 * kPi;
const double kPi2 = kPi / 2;

vector<float> getLowPassFIRCoeffs(int sampleRate, float halfAmplFreq,
                                  int length) {
  length += (length + 1) % 2;
  float freq = halfAmplFreq / sampleRate;
  int center = length / 2;
  float sum = 0;
  vector<float> coefficients(length);
  for (int i = 0; i < length; ++i) {
    float val;
    if (i == center) {
      val = k2Pi * freq;
    } else {
      float angle = k2Pi * (i + 1) / (length + 1);
      val = sin(k2Pi * freq * (i - center)) / (i - center);
      val *= 0.42 - 0.5 * cos(angle) + 0.08 * cos(2 * angle);
    }
    sum += val;
    coefficients[i] = val;
  }
  for (int i = 0; i < length; ++i) {
    coefficients[i] /= sum;
  }
  return coefficients;
}

Samples samplesFromUint8(uint8_t* buffer, int length) {
  Samples out(length);
  for (int i = 0; i < length; ++i) {
    out[i] = buffer[i] / 128.0 - 1;
  }
  return out;
}

FIRFilter::FIRFilter(const vector<float>& coefficients, int step)
    : coefficients_(coefficients),
      curSamples_((coefficients.size() - 1) * step, 0),
      step_(step), offset_((coefficients.size() - 1) * step) {
  reverse(coefficients_.begin(), coefficients_.end());
}

void FIRFilter::loadSamples(const Samples& samples) {
  int fullLen = samples.size() + offset_;
  float* curArr = curSamples_.data();
  float* endOfCur = curArr + curSamples_.size() - offset_;
  memmove(curArr, endOfCur, offset_ * sizeof(float));
  if (fullLen != curSamples_.size()) {
    curSamples_.resize(fullLen);
    curArr = curSamples_.data();
  }
  memmove(curArr + offset_, samples.data(), samples.size() * sizeof(float));
}

float FIRFilter::get(int index) {
  float out = 0;
  for (int ic = 0, is = index, sz = coefficients_.size(); ic < sz;
       ++ic, is += step_) {
    out += coefficients_[ic] * curSamples_[is];
  }
  return out;
}

Downsampler::Downsampler(int inRate, int outRate,
                         const vector<float>& coefs)
    : filter_(coefs, 1), rateMul_((float) inRate / outRate) {}

Samples Downsampler::downsample(const Samples& samples) {
  filter_.loadSamples(samples);
  int outLen = samples.size() / rateMul_;
  Samples out(outLen);
  float readFrom = 0;
  for (int i = 0; i < outLen; ++i, readFrom += rateMul_) {
    out[i] = filter_.get((int) readFrom);
  }
  return out;
}

IQDownsampler::IQDownsampler(int inRate, int outRate,
                             const vector<float>& coefs)
    : filter_(coefs, 2), rateMul_((float) inRate / outRate) {}

SamplesIQ IQDownsampler::downsample(const Samples& samples) {
  int numSamples = samples.size() / (2 * rateMul_);
  filter_.loadSamples(samples);
  SamplesIQ out{Samples(numSamples), Samples(numSamples)};
  float readFrom = 0;
  for (int i = 0; i < numSamples; ++i, readFrom += rateMul_) {
    int idx = 2 * ((int) readFrom);
    out.I[i] = filter_.get(idx);
    out.Q[i] = filter_.get(idx + 1);
  }
  return out;
}

AMDemodulator::AMDemodulator(int inRate, int outRate, float filterFreq,
                             int kernelLen)
    : downsampler_(inRate, outRate,
                   getLowPassFIRCoeffs(inRate, filterFreq, kernelLen)) {}

Samples AMDemodulator::demodulateTuned(const Samples& samples) {
  SamplesIQ iqSamples(downsampler_.downsample(samples));
  int outLen = iqSamples.I.size();
  float iAvg = accumulate(iqSamples.I.begin(), iqSamples.I.end(), 0) / outLen;
  float qAvg = accumulate(iqSamples.Q.begin(), iqSamples.Q.end(), 0) / outLen;
  Samples out(outLen);
  float sigSqrSum = 0;
  float sigSum = 0;
  for (int i = 0; i < outLen; ++i) {
    float I = iqSamples.I[i] - iAvg;
    float Q = iqSamples.Q[i] - qAvg;
    float power = I * I + Q * Q;
    float ampl = sqrt(power);
    out[i] = ampl;
    sigSum += ampl;
    sigSqrSum += power;
  }
  float halfPoint = sigSum / outLen;
  for (auto &o : out) {
    o = (o - halfPoint) / halfPoint;
  }
  hasCarrier_ = sigSqrSum > (0.002 * outLen);
  return out;
}

bool AMDemodulator::hasCarrier() {
  return hasCarrier_;
}

static float myatan2(float y, float x) {
  float sgn = 1;
  if (y < 0) {
    sgn *= -1;
    y *= -1;
  }
  float ang = 0;
  float div;
  if (x == y) {
    div = 1;
  } else if (x > y) {
    div = y / x;
  } else {
    ang = -kPi2;
    div = x / y;
    sgn *= -1;
  }
  ang +=
    div /
    (0.98419158358617365
     + div * (0.093485702629671305
	      + div * 0.19556307900617517));
  return sgn * ang;
}

FMDemodulator::FMDemodulator(int inRate, int outRate, int maxF,
                             float filterFreq, int kernelLen)
  : amplConv_(outRate / (k2Pi * maxF)),
    downsampler_(inRate, outRate,
                 getLowPassFIRCoeffs(inRate, filterFreq, kernelLen)),
    lI_(0), lQ_(0) {}

Samples FMDemodulator::demodulateTuned(const Samples& samples) {
  SamplesIQ iqSamples(downsampler_.downsample(samples));
  int outLen = iqSamples.I.size();
  Samples out(outLen);
  float sigSqrSum = 0;
  for (int i = 0; i < outLen; ++i) {
    float I = iqSamples.I[i];
    float Q = iqSamples.Q[i];
    float real = lI_ * I + lQ_ * Q;
    float imag = lI_ * Q - I * lQ_;
    out[i] = myatan2(imag, real) * amplConv_;
    lI_ = I;
    lQ_ = Q;
    sigSqrSum += lI_ * lI_;
  }
  hasCarrier_ = sigSqrSum > (0.002 * outLen);
  return out;
}

bool FMDemodulator::hasCarrier() {
  return hasCarrier_;
}

class StereoSeparator::ExpAverage {
  float weight_;
  float avg_;

 public:
  ExpAverage(int weight) : weight_(weight), avg_(0) {}

  float add(float value) {
    avg_ = (weight_ * avg_ + value) / (weight_ + 1);
    return avg_;
  }

  float get() { return avg_; }
};

StereoSeparator::StereoSeparator(int sampleRate, int pilotFreq)
    : sin_(0), cos_(1),
      iavg_(new ExpAverage(sampleRate * 0.03)),
      qavg_(new ExpAverage(sampleRate * 0.03)),
      cavg_(new ExpAverage(sampleRate * 0.15)) {
  for (int i = 0; i < 8001; ++i) {
    float freq = (pilotFreq + i / 100 - 40) * k2Pi / sampleRate;
    sinTable_[i] = sin(freq);
    cosTable_[i] = cos(freq);
  }
}

StereoSeparator::~StereoSeparator() {}

const float StereoSeparator::kCorrThres = 4;

StereoSignal StereoSeparator::separate(const Samples& samples) {
  Samples out(samples);
  for (int i = 0, sz = out.size(); i < sz; ++i) {
    float hdev = qavg_->add(out[i] * cos_);
    float vdev = iavg_->add(out[i] * sin_);
    out[i] *= sin_ * cos_ * 2;
    float corr;
    if (vdev > 0) {
      corr = fmaxf(-4, fminf(4, hdev / vdev));
    } else {
      corr = hdev == 0 ? 0 : hdev > 0 ? 4 : -4;
    }
    int idx = roundf((corr + 4) * 1000);
    float newSin = sin_ * cosTable_[idx] + cos_ * sinTable_[idx];
    cos_ = cos_ * cosTable_[idx] - sin_ * sinTable_[idx];
    sin_ = newSin;
    cavg_->add(corr * corr);
  }

  return StereoSignal{cavg_->get() < kCorrThres, out};
}

Deemphasizer::Deemphasizer(int sampleRate, int timeConstant_uS)
  : mult_(exp(-1e6 / (timeConstant_uS * sampleRate))), val_(0) {}

void Deemphasizer::inPlace(Samples& samples) {
  for (int i = 0, sz = samples.size(); i < sz; ++i) {
    val_ = (1 - mult_) * samples[i] + mult_ * val_;
    samples[i] = val_;
  }
}

}  // namespace radioreceiver
<commit_msg>Add missing header.<commit_after>// Copyright 2014 Google Inc. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/**
 * DSP functions and operations.
 */

#include <algorithm>
#include <cassert>
#include <cmath>
#include <cstring>
#include <memory>
#include <numeric>
#include <stdint.h>
#include <vector>

#include "dsp.h"

using namespace std;

namespace radioreceiver {

const double kPi = 3.141592653989793238;
const double k2Pi = 2 * kPi;
const double kPi2 = kPi / 2;

vector<float> getLowPassFIRCoeffs(int sampleRate, float halfAmplFreq,
                                  int length) {
  length += (length + 1) % 2;
  float freq = halfAmplFreq / sampleRate;
  int center = length / 2;
  float sum = 0;
  vector<float> coefficients(length);
  for (int i = 0; i < length; ++i) {
    float val;
    if (i == center) {
      val = k2Pi * freq;
    } else {
      float angle = k2Pi * (i + 1) / (length + 1);
      val = sin(k2Pi * freq * (i - center)) / (i - center);
      val *= 0.42 - 0.5 * cos(angle) + 0.08 * cos(2 * angle);
    }
    sum += val;
    coefficients[i] = val;
  }
  for (int i = 0; i < length; ++i) {
    coefficients[i] /= sum;
  }
  return coefficients;
}

Samples samplesFromUint8(uint8_t* buffer, int length) {
  Samples out(length);
  for (int i = 0; i < length; ++i) {
    out[i] = buffer[i] / 128.0 - 1;
  }
  return out;
}

FIRFilter::FIRFilter(const vector<float>& coefficients, int step)
    : coefficients_(coefficients),
      curSamples_((coefficients.size() - 1) * step, 0),
      step_(step), offset_((coefficients.size() - 1) * step) {
  reverse(coefficients_.begin(), coefficients_.end());
}

void FIRFilter::loadSamples(const Samples& samples) {
  int fullLen = samples.size() + offset_;
  float* curArr = curSamples_.data();
  float* endOfCur = curArr + curSamples_.size() - offset_;
  memmove(curArr, endOfCur, offset_ * sizeof(float));
  if (fullLen != curSamples_.size()) {
    curSamples_.resize(fullLen);
    curArr = curSamples_.data();
  }
  memmove(curArr + offset_, samples.data(), samples.size() * sizeof(float));
}

float FIRFilter::get(int index) {
  float out = 0;
  for (int ic = 0, is = index, sz = coefficients_.size(); ic < sz;
       ++ic, is += step_) {
    out += coefficients_[ic] * curSamples_[is];
  }
  return out;
}

Downsampler::Downsampler(int inRate, int outRate,
                         const vector<float>& coefs)
    : filter_(coefs, 1), rateMul_((float) inRate / outRate) {}

Samples Downsampler::downsample(const Samples& samples) {
  filter_.loadSamples(samples);
  int outLen = samples.size() / rateMul_;
  Samples out(outLen);
  float readFrom = 0;
  for (int i = 0; i < outLen; ++i, readFrom += rateMul_) {
    out[i] = filter_.get((int) readFrom);
  }
  return out;
}

IQDownsampler::IQDownsampler(int inRate, int outRate,
                             const vector<float>& coefs)
    : filter_(coefs, 2), rateMul_((float) inRate / outRate) {}

SamplesIQ IQDownsampler::downsample(const Samples& samples) {
  int numSamples = samples.size() / (2 * rateMul_);
  filter_.loadSamples(samples);
  SamplesIQ out{Samples(numSamples), Samples(numSamples)};
  float readFrom = 0;
  for (int i = 0; i < numSamples; ++i, readFrom += rateMul_) {
    int idx = 2 * ((int) readFrom);
    out.I[i] = filter_.get(idx);
    out.Q[i] = filter_.get(idx + 1);
  }
  return out;
}

AMDemodulator::AMDemodulator(int inRate, int outRate, float filterFreq,
                             int kernelLen)
    : downsampler_(inRate, outRate,
                   getLowPassFIRCoeffs(inRate, filterFreq, kernelLen)) {}

Samples AMDemodulator::demodulateTuned(const Samples& samples) {
  SamplesIQ iqSamples(downsampler_.downsample(samples));
  int outLen = iqSamples.I.size();
  float iAvg = accumulate(iqSamples.I.begin(), iqSamples.I.end(), 0) / outLen;
  float qAvg = accumulate(iqSamples.Q.begin(), iqSamples.Q.end(), 0) / outLen;
  Samples out(outLen);
  float sigSqrSum = 0;
  float sigSum = 0;
  for (int i = 0; i < outLen; ++i) {
    float I = iqSamples.I[i] - iAvg;
    float Q = iqSamples.Q[i] - qAvg;
    float power = I * I + Q * Q;
    float ampl = sqrt(power);
    out[i] = ampl;
    sigSum += ampl;
    sigSqrSum += power;
  }
  float halfPoint = sigSum / outLen;
  for (auto &o : out) {
    o = (o - halfPoint) / halfPoint;
  }
  hasCarrier_ = sigSqrSum > (0.002 * outLen);
  return out;
}

bool AMDemodulator::hasCarrier() {
  return hasCarrier_;
}

static float myatan2(float y, float x) {
  float sgn = 1;
  if (y < 0) {
    sgn *= -1;
    y *= -1;
  }
  float ang = 0;
  float div;
  if (x == y) {
    div = 1;
  } else if (x > y) {
    div = y / x;
  } else {
    ang = -kPi2;
    div = x / y;
    sgn *= -1;
  }
  ang +=
    div /
    (0.98419158358617365
     + div * (0.093485702629671305
	      + div * 0.19556307900617517));
  return sgn * ang;
}

FMDemodulator::FMDemodulator(int inRate, int outRate, int maxF,
                             float filterFreq, int kernelLen)
  : amplConv_(outRate / (k2Pi * maxF)),
    downsampler_(inRate, outRate,
                 getLowPassFIRCoeffs(inRate, filterFreq, kernelLen)),
    lI_(0), lQ_(0) {}

Samples FMDemodulator::demodulateTuned(const Samples& samples) {
  SamplesIQ iqSamples(downsampler_.downsample(samples));
  int outLen = iqSamples.I.size();
  Samples out(outLen);
  float sigSqrSum = 0;
  for (int i = 0; i < outLen; ++i) {
    float I = iqSamples.I[i];
    float Q = iqSamples.Q[i];
    float real = lI_ * I + lQ_ * Q;
    float imag = lI_ * Q - I * lQ_;
    out[i] = myatan2(imag, real) * amplConv_;
    lI_ = I;
    lQ_ = Q;
    sigSqrSum += lI_ * lI_;
  }
  hasCarrier_ = sigSqrSum > (0.002 * outLen);
  return out;
}

bool FMDemodulator::hasCarrier() {
  return hasCarrier_;
}

class StereoSeparator::ExpAverage {
  float weight_;
  float avg_;

 public:
  ExpAverage(int weight) : weight_(weight), avg_(0) {}

  float add(float value) {
    avg_ = (weight_ * avg_ + value) / (weight_ + 1);
    return avg_;
  }

  float get() { return avg_; }
};

StereoSeparator::StereoSeparator(int sampleRate, int pilotFreq)
    : sin_(0), cos_(1),
      iavg_(new ExpAverage(sampleRate * 0.03)),
      qavg_(new ExpAverage(sampleRate * 0.03)),
      cavg_(new ExpAverage(sampleRate * 0.15)) {
  for (int i = 0; i < 8001; ++i) {
    float freq = (pilotFreq + i / 100 - 40) * k2Pi / sampleRate;
    sinTable_[i] = sin(freq);
    cosTable_[i] = cos(freq);
  }
}

StereoSeparator::~StereoSeparator() {}

const float StereoSeparator::kCorrThres = 4;

StereoSignal StereoSeparator::separate(const Samples& samples) {
  Samples out(samples);
  for (int i = 0, sz = out.size(); i < sz; ++i) {
    float hdev = qavg_->add(out[i] * cos_);
    float vdev = iavg_->add(out[i] * sin_);
    out[i] *= sin_ * cos_ * 2;
    float corr;
    if (vdev > 0) {
      corr = fmaxf(-4, fminf(4, hdev / vdev));
    } else {
      corr = hdev == 0 ? 0 : hdev > 0 ? 4 : -4;
    }
    int idx = roundf((corr + 4) * 1000);
    float newSin = sin_ * cosTable_[idx] + cos_ * sinTable_[idx];
    cos_ = cos_ * cosTable_[idx] - sin_ * sinTable_[idx];
    sin_ = newSin;
    cavg_->add(corr * corr);
  }

  return StereoSignal{cavg_->get() < kCorrThres, out};
}

Deemphasizer::Deemphasizer(int sampleRate, int timeConstant_uS)
  : mult_(exp(-1e6 / (timeConstant_uS * sampleRate))), val_(0) {}

void Deemphasizer::inPlace(Samples& samples) {
  for (int i = 0, sz = samples.size(); i < sz; ++i) {
    val_ = (1 - mult_) * samples[i] + mult_ * val_;
    samples[i] = val_;
  }
}

}  // namespace radioreceiver
<|endoftext|>


<commit_before>#include <stan/prob/distributions/univariate/continuous/student_t.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include<boost/math/distributions.hpp>

TEST(ProbDistributionsStudentT, random) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::prob::student_t_rng(3.0, 2.0, 1.0, rng));
}

TEST(ProbDistributionsStudentT, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  boost::math::students_t_distribution<>dist (3.0);
  int N = 10000;
  int K = 5;
  boost::math::chi_squared mydist(K-1);

  double loc[4];
  for(int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, 0.2 * i);

  int count = 0;
  int bin [5] = {0, 0, 0, 0, 0};

  while (count < N) {
    double a = stan::prob::student_t_rng(3.0,2.0,1.0,rng) / 1.0 - 2.0;
    int i = 0;
    while (i < K-1 && a > loc[i])
  ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;
  double expect [5] = {N / K, N / K, N / K, N / K, N / K};

  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}
<commit_msg>fixed chi square test for rng for student t<commit_after>#include <stan/prob/distributions/univariate/continuous/student_t.hpp>
#include <gtest/gtest.h>
#include <boost/random/mersenne_twister.hpp>
#include<boost/math/distributions.hpp>

TEST(ProbDistributionsStudentT, random) {
  boost::random::mt19937 rng;
  EXPECT_NO_THROW(stan::prob::student_t_rng(3.0, 2.0, 2.0, rng));
}

TEST(ProbDistributionsStudentT, chiSquareGoodnessFitTest) {
  boost::random::mt19937 rng;
  boost::math::students_t_distribution<>dist (3.0);
  int N = 10000;
  int K = 5;
  boost::math::chi_squared mydist(K-1);

  double loc[4];
  for(int i = 1; i < K; i++)
    loc[i - 1] = quantile(dist, 0.2 * i);

  int count = 0;
  int bin [5] = {0, 0, 0, 0, 0};

  while (count < N) {
    double a = (stan::prob::student_t_rng(3.0,2.0,2.0,rng) - 2.0) / 2.0;
    int i = 0;
    while (i < K-1 && a > loc[i])
      ++i;
    ++bin[i];
    count++;
   }

  double chi = 0;
  double expect [5] = {N / K, N / K, N / K, N / K, N / K};

  for(int j = 0; j < K; j++)
    chi += ((bin[j] - expect[j]) * (bin[j] - expect[j]) / expect[j]);

  EXPECT_TRUE(chi < quantile(complement(mydist, 1e-6)));
}
<|endoftext|>


<commit_before>#include <string>
#include <vector>
#include <iostream>
#include <algorithm>
#include <ctime>

namespace nmea_parser {

#define COMMANDSTART 0x24
#define SENTENCESEPARATOR 0x2C
#define CHECKSUMSTART 0x2A

class NMEAData {
public:
  NMEAData(){};
  NMEAData(const std::string TalkerID, const std::string MessageType) {
    ID = TalkerID;
    Type = MessageType;
  };
  std::string GetType() { return Type; };
  std::string GetTalkerID() { return ID; };
  std::string Print() {
    std::string Result;

    Result = "Talker ID: ";
    Result += ID + "\n";

    Result += "Message Type: ";
    Result += Type + "\n";

    return Result;
  };

private:
  std::string ID;
  std::string Type;
};

class GPRMC : NMEAData {
public:
  GPRMC(const std::string TalkerID, const std::string MessageType,
        time_t TimeStamp, char Status, float Latitude, char LatitudeDirection,
        float Longitude, char LongitudeDirection, float Speed, float Angle,
        int Date, float MagneticVariation, char MagneticVariationDirection,
        std::string Checksum)
      : NMEAData(TalkerID, MessageType), TimeStamp(TimeStamp), Status(Status),
        Latitude(Latitude), LatitudeDirection(LatitudeDirection),
        Longitude(Longitude), LongitudeDirection(LongitudeDirection),
        Speed(Speed), Angle(Angle), Date(Date),
        MagneticVariation(MagneticVariation),
        MagneticVariationDirection(MagneticVariationDirection),
        Checksum(Checksum){};

private:
  // Timestamp when fix was taken
  time_t TimeStamp;
  // Status A = active, V = void
  char Status;
  // Latitude of position:
  // e.g. 4807.038,N = 48 deg 07.038' N
  float Latitude;
  char LatitudeDirection;
  // Longitude of position:
  // e.g. 01131.000,E = 11 deg 31.000' E
  float Longitude;
  char LongitudeDirection;
  // Speed over the ground in knots
  float Speed;
  // Track angle in degrees True
  float Angle;
  // Date - date stamp
  // e.g. 230394 = 23rd of March 1994
  int Date;
  // Magnetic Variation
  // e.g. 003.1,W
  float MagneticVariation;
  char MagneticVariationDirection;
  // Checksum data
  // e.g. *6A
  std::string Checksum;
};

class NMEAParser {
public:
  NMEAParser() {}

  bool ValidateChecksum(const std::string *Message,
                        const std::string *Checksum) {
    unsigned int CalculatedChecksum = 0;
    unsigned int Check = 0;

    for (unsigned int i = 1; i < (Message->length() - 3); ++i) {
      CalculatedChecksum ^= Message->at(i);
    }

    Check = std::stoul(*Checksum, nullptr, 16);

    if (Check == CalculatedChecksum)
      return true;

    return false;
  }

  NMEAData parse(const std::string *str) {
    NMEAData Result;

    // Strings starting without $ are invalid
    if (COMMANDSTART != str->at(0))
      return NMEAData("NONE", "INVALID");

    // TODO: Handle message from GLONASS
    if (('G' != str->at(1)) || ('P' != str->at(2)))
      return NMEAData("NONE", "INVALID");

    // TODO: Check if a command is longer or shorter than 3 characters
    if (str->at(6) != SENTENCESEPARATOR)
      return NMEAData("NONE", "INVALID");

    std::string TalkerID = str->substr(1, 2);
    std::string MessageType = str->substr(3, 3);

    if ("RMC" == MessageType) {
      std::string DataSentence = str->substr(7, str->length() - 4);
      const std::string Checksum =
          str->substr(str->length() - 2, (str->length() - 1));

      // Validate Checksum
      if (!ValidateChecksum(str, &Checksum))
        return NMEAData("NONE", "INVALID");

    }

    // Result = NMEAData(TalkerID, MessageType);

    return Result;
  }
};
};
<commit_msg>Test implement time stamp parsing<commit_after>#include <string>
#include <vector>
#include <iostream>
#include <algorithm>
#include <ctime>

namespace nmea_parser {

#define COMMANDSTART 0x24
#define SENTENCESEPARATOR 0x2C
#define CHECKSUMSTART 0x2A

class NMEAData {
public:
  NMEAData(){};
  NMEAData(const std::string TalkerID, const std::string MessageType) {
    ID = TalkerID;
    Type = MessageType;
  };
  std::string GetType() { return Type; };
  std::string GetTalkerID() { return ID; };
  std::string Print() {
    std::string Result;

    Result = "Talker ID: ";
    Result += ID + "\n";

    Result += "Message Type: ";
    Result += Type + "\n";

    return Result;
  };

private:
  std::string ID;
  std::string Type;
};

class GPRMC : NMEAData {
public:
  GPRMC(const std::string TalkerID, const std::string MessageType,
        time_t TimeStamp, char Status, float Latitude, char LatitudeDirection,
        float Longitude, char LongitudeDirection, float Speed, float Angle,
        int Date, float MagneticVariation, char MagneticVariationDirection,
        std::string Checksum)
      : NMEAData(TalkerID, MessageType), TimeStamp(TimeStamp), Status(Status),
        Latitude(Latitude), LatitudeDirection(LatitudeDirection),
        Longitude(Longitude), LongitudeDirection(LongitudeDirection),
        Speed(Speed), Angle(Angle), Date(Date),
        MagneticVariation(MagneticVariation),
        MagneticVariationDirection(MagneticVariationDirection),
        Checksum(Checksum){};

private:
  // Timestamp when fix was taken
  time_t TimeStamp;
  // Status A = active, V = void
  char Status;
  // Latitude of position:
  // e.g. 4807.038,N = 48 deg 07.038' N
  float Latitude;
  char LatitudeDirection;
  // Longitude of position:
  // e.g. 01131.000,E = 11 deg 31.000' E
  float Longitude;
  char LongitudeDirection;
  // Speed over the ground in knots
  float Speed;
  // Track angle in degrees True
  float Angle;
  // Date - date stamp
  // e.g. 230394 = 23rd of March 1994
  int Date;
  // Magnetic Variation
  // e.g. 003.1,W
  float MagneticVariation;
  char MagneticVariationDirection;
  // Checksum data
  // e.g. *6A
  std::string Checksum;
};

class NMEAParser {
public:
  NMEAParser() {}

  bool ValidateChecksum(const std::string *Message,
                        const std::string *Checksum) {
    unsigned int CalculatedChecksum = 0;
    unsigned int Check = 0;

    for (unsigned int i = 1; i < (Message->length() - 3); ++i) {
      CalculatedChecksum ^= Message->at(i);
    }

    Check = std::stoul(*Checksum, nullptr, 16);

    if (Check == CalculatedChecksum)
      return true;

    return false;
  }

  int GPRMCParseTimestamp(std::string *DataSentence) {
    time_t Result;
    struct tm *TimeInfo;

    const std::string StringHour = DataSentence->substr(0, 2);
    const std::string StringMinute = DataSentence->substr(2, 2);
    const std::string StringSecond = DataSentence->substr(4, 2);

    int Hour = std::stoi(StringHour, nullptr, 10);
    int Minute = std::stoi(StringMinute, nullptr, 10);
    int Second = std::stoi(StringSecond, nullptr, 10);

    time(&Result);
    // TODO: Do we want local time?
    TimeInfo = localtime(&Result);
    TimeInfo->tm_hour = Hour;
    TimeInfo->tm_min = Minute;
    TimeInfo->tm_sec = Second;
    Result = mktime(TimeInfo);

    char buffer[80];
    strftime(buffer, 80, "Time: %F %T", TimeInfo);
    puts(buffer);

    return Result;
  };

  NMEAData parse(const std::string *str) {
    NMEAData Result;

    // Strings starting without $ are invalid
    if (COMMANDSTART != str->at(0))
      return NMEAData("NONE", "INVALID");

    // TODO: Handle message from GLONASS
    if (('G' != str->at(1)) || ('P' != str->at(2)))
      return NMEAData("NONE", "INVALID");

    // TODO: Check if a command is longer or shorter than 3 characters
    if (str->at(6) != SENTENCESEPARATOR)
      return NMEAData("NONE", "INVALID");

    std::string TalkerID = str->substr(1, 2);
    std::string MessageType = str->substr(3, 3);

    if ("RMC" == MessageType) {
      std::string DataSentence = str->substr(7, str->length() - 4);
      const std::string Checksum =
          str->substr(str->length() - 2, (str->length() - 1));

      // Validate Checksum
      if (!ValidateChecksum(str, &Checksum))
        return NMEAData("NONE", "INVALID");

      GPRMCParseTimestamp(&DataSentence);
    }

    // Result = NMEAData(TalkerID, MessageType);

    return Result;
  }
};
};
<|endoftext|>


<commit_before>/**
*  Copyright (C) 2019 3D Repo Ltd
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU Affero General Public License as
*  published by the Free Software Foundation, either version 3 of the
*  License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU Affero General Public License for more details.
*
*  You should have received a copy of the GNU Affero General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <gtest/gtest.h>
#include <repo/core/handler/fileservice/repo_file_manager.h>
#include <repo/core/model/bson/repo_node.h>
#include <repo/lib/repo_exception.h>
#include <repo/lib/repo_utils.h>
#include "../../../../repo_test_fileservice_info.h"

using namespace repo::core::handler::fileservice;

static FileManager* getManagerDefaultFS()
{
	FileManager::disconnect();
	return FileManager::instantiateManager(repo::lib::RepoConfig::fromFile(getDataPath("config/withFS.json")), getHandler());
}

TEST(FileManager, GetManager)
{
	FileManager::disconnect();
	EXPECT_THROW(FileManager::getManager(), repo::lib::RepoException);
	EXPECT_NO_THROW(getManagerDefaultFS());
}

TEST(FileManager, InstantiateManager)
{
	FileManager::disconnect();
	EXPECT_THROW(FileManager::instantiateManager(repo::lib::RepoConfig::fromFile(getDataPath("config/withFS.json")), nullptr), repo::lib::RepoException);
}
<commit_msg>ISSUE #320 uploadFileAndCommit test<commit_after>/**
*  Copyright (C) 2019 3D Repo Ltd
*
*  This program is free software: you can redistribute it and/or modify
*  it under the terms of the GNU Affero General Public License as
*  published by the Free Software Foundation, either version 3 of the
*  License, or (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful,
*  but WITHOUT ANY WARRANTY; without even the implied warranty of
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
*  GNU Affero General Public License for more details.
*
*  You should have received a copy of the GNU Affero General Public License
*  along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

#include <gtest/gtest.h>
#include <repo/core/handler/fileservice/repo_file_manager.h>
#include <repo/core/model/bson/repo_node.h>

#include <repo/lib/repo_exception.h>
#include <repo/lib/repo_utils.h>
#include "../../../../repo_test_fileservice_info.h"

using namespace repo::core::handler::fileservice;

static FileManager* getManagerDefaultFS()
{
	FileManager::disconnect();
	auto config = repo::lib::RepoConfig::fromFile(getDataPath("config/withFS.json"));
	config.configureFS(getDataPath("fileShare"));
	return FileManager::instantiateManager(config, getHandler());
}

TEST(FileManager, GetManager)
{
	FileManager::disconnect();
	EXPECT_THROW(FileManager::getManager(), repo::lib::RepoException);
	EXPECT_NO_THROW(getManagerDefaultFS());
}

TEST(FileManager, InstantiateManager)
{
	FileManager::disconnect();
	EXPECT_THROW(FileManager::instantiateManager(repo::lib::RepoConfig::fromFile(getDataPath("config/withFS.json")), nullptr), repo::lib::RepoException);
}

TEST(FileManager, UploadFileAndCommit)
{
	auto manager = getManagerDefaultFS();
	ASSERT_TRUE(manager);
	auto db = "testFileManager";
	std::string col = "fileUpload";
	auto fileName = "fileTest";
	std::vector<uint8_t> file;
	file.resize(1024);
	EXPECT_TRUE(manager->uploadFileAndCommit(db, col, fileName, file));

	auto dbHandler = getHandler();
	auto res = dbHandler->findOneByCriteria(db, col + "." + REPO_COLLECTION_EXT_REF, BSON("_id" << fileName));
	EXPECT_FALSE(res.isEmpty());
	std::string linkName = res.getStringField(REPO_REF_LABEL_LINK);
	EXPECT_TRUE(repo::lib::doesFileExist(getDataPath("fileShare/" + linkName)));
}
<|endoftext|>


<commit_before>#include <v8.h>
#include <node.h>
#include <node_buffer.h>

#include <string>

#include "markdownWrapper.hpp"

using namespace std;

using namespace node;
using namespace v8;

using namespace mkd;

#define OUTPUT_UNIT 64

// Credit: @samcday
// http://sambro.is-super-awesome.com/2011/03/03/creating-a-proper-buffer-in-a-node-c-addon/
#define MAKE_FAST_BUFFER(NG_SLOW_BUFFER, NG_FAST_BUFFER)      \
  Local<Function> NG_JS_BUFFER = Local<Function>::Cast(       \
    Context::GetCurrent()->Global()->Get(                     \
      String::New("Buffer")));                                \
                                                              \
  Handle<Value> NG_JS_ARGS[3] = {                             \
    NG_SLOW_BUFFER->handle_,                                  \
    Integer::New(Buffer::Length(NG_SLOW_BUFFER)),             \
    Integer::New(0)                                           \
  };                                                          \
                                                              \
  NG_FAST_BUFFER = NG_JS_BUFFER->NewInstance(3, NG_JS_ARGS);

struct request {
  Persistent<Function> callback;
  string in;
  buf *out; //Pass the output buffer directly
  unsigned int flags;
  Persistent<Object> renderer;
};

////////////////////////
// Renderer functions //
////////////////////////

class Renderer : public ObjectWrap {
protected:
  mkd_renderer* const ptr;
public:
  inline Renderer() : ptr(new mkd_renderer) {}
  inline mkd_renderer* getHandle() {return ptr;}
  inline ~Renderer() {
    delete ptr;
  }
  static Handle<Value> newInstance(const Arguments& args);
};

class HtmlRenderer : public Renderer {
public:
  inline HtmlRenderer(unsigned int flags) {
    upshtml_renderer(ptr, flags);
  }
  inline ~HtmlRenderer() {
    upshtml_free_renderer(ptr);
  }
  static Handle<Value> newInstance(const Arguments& args);
};

Handle<Value> Renderer::newInstance(const Arguments& args) {
  HandleScope scope;

  (new Renderer)->Wrap(args.This());
  return scope.Close(args.This());
}

Handle<Value> HtmlRenderer::newInstance(const Arguments& args) {
  HandleScope scope;

  unsigned int flags = 0;
  if (args.Length() >= 1) flags = args[0]->Uint32Value(); //FIXME: introduce method, allow list of flags

  (new HtmlRenderer(flags))->Wrap(args.This());
  return scope.Close(args.This());
}

//////////////////////
// Render functions //
//////////////////////

static void markdownProcess(eio_req *req);
static int markdownAfter(eio_req *req);

static void outputBufferFree(char* data, void* hint) {
  buf* b = static_cast<buf*>(hint);
  bufrelease(b);
}

static Handle<Value> markdown(const Arguments& args) {
  HandleScope scope;
  try {
    //Check arguments
    if (args.Length() < 3 || (!args[0]->IsObject()) || (!args[2]->IsFunction()))
      throw Exception::TypeError(String::New("usage: markdown(renderer, input, callback, [extensions])"));

    //Extract arguments
    Local<Object> rend = args[0]->ToObject();
    String::Utf8Value in(args[1]);
    Local<Function> callback = Local<Function>::Cast(args[2]);
    unsigned int flags = 0;
    if (args.Length() >= 4) flags = args[3]->Uint32Value(); //FIXME

    //Make request
    request *sr = new request;
    sr->callback = Persistent<Function>::New(callback);
    sr->in = *in;
    sr->flags = flags;
    sr->renderer = Persistent<Object>::New(rend);

    //Call process
    eio_custom(markdownProcess, EIO_PRI_DEFAULT, markdownAfter, sr);
    ev_ref(EV_DEFAULT_UC);

    return scope.Close( Undefined() );
  } catch (Handle<Value> h) {
    return ThrowException(h);
  } catch (exception e) {
    return ThrowException(Exception::Error(String::New(e.what())));
  }
}

static void markdownProcess(eio_req *req) {
  //Extract request and renderer
  request *sr = static_cast<request *>(req->data);
  Renderer* rwrap = Renderer::Unwrap<Renderer>(sr->renderer);
  mkd_renderer* rend = rwrap->getHandle();

  // Create input buffer from string
  struct buf *input_buf = bufnew(sr->in.size());
//  try {FIXME: RAII
    bufput(input_buf, sr->in.c_str(), sr->in.size());

    // Create output buffer and reset size to 0
    sr->out = bufnew(OUTPUT_UNIT);
    sr->out->size = 0;

    // Render!
    ups_markdown(sr->out, input_buf, rend, sr->flags);
//  } finally {
    //Free input buffer
    bufrelease(input_buf);
//  }
}

static int markdownAfter(eio_req *req) {
  HandleScope scope;

  ev_unref(EV_DEFAULT_UC);
  request *sr = static_cast<request*>(req->data);
  Local<Value> argv[1];

  // Create and assign fast buffer to arguments array
  Buffer* buffer = Buffer::New(const_cast<char *>(sr->out->data), sr->out->size, outputBufferFree, sr->out);
  Local<Object> fastBuffer;
  MAKE_FAST_BUFFER(buffer, fastBuffer);
  argv[0] = fastBuffer;

  // Invoke callback function with html argument
  TryCatch try_catch;
  sr->callback->Call(Context::GetCurrent()->Global(), 1, argv);
  if (try_catch.HasCaught()) {
    FatalException(try_catch);
  }

  /* cleanup */
  sr->callback.Dispose(); //FIXME: should we put this into a finally (RAII)?
  return 0;
}

static Handle<Value> markdownSync(const Arguments &args) {
  HandleScope scope;
  try {
    //Check arguments
    if (args.Length() < 2 || (!args[0]->IsObject()))
      throw Exception::TypeError(String::New("usage: markdown(renderer, input, [extensions])"));

    //Extract arguments
    Renderer* rwrap = Renderer::Unwrap<Renderer>(args[0]->ToObject());
    mkd_renderer* rend = rwrap->getHandle();
    String::Utf8Value instr (args[1]);
    unsigned int flags = 0;
    if (args.Length() >= 3) flags = args[2]->Uint32Value(); //FIXME

    // Create input buffer from string
    struct buf *input_buf = bufnew(instr.length());
  //  try {FIXME: RAII
      bufput(input_buf, *instr, instr.length());

      // Create output buffer and reset size to 0
      struct buf *out = bufnew(OUTPUT_UNIT);
      out->size = 0;

      // Render!
      ups_markdown(out, input_buf, rend, flags);

      // Create and return fast buffer to result
      Buffer* buffer = Buffer::New(const_cast<char *>(out->data), out->size, outputBufferFree, out);
      Local<Object> fastBuffer;
      MAKE_FAST_BUFFER(buffer, fastBuffer);
  //  } finally {
      //Free input buffer
      bufrelease(input_buf);
  //  }
      return scope.Close(fastBuffer);
  } catch (Handle<Value> h) {
    return ThrowException(h);
  } catch (exception e) {
    return ThrowException(Exception::Error(String::New(e.what())));
  }
}

////////////////////////
// Module declaration //
////////////////////////

extern "C" {
  void init (Handle<Object> target) {
    HandleScope scope;

    //Static functions & properties
    target->Set(String::NewSymbol("version"), String::New("1.0.0"));
    NODE_SET_METHOD(target, "markdown", markdown);
    NODE_SET_METHOD(target, "markdownSync", markdownSync);

    //Extension constants
    target->Set(String::NewSymbol("EXT_AUTOLINK"), Integer::New(MKDEXT_AUTOLINK));
    target->Set(String::NewSymbol("EXT_FENCED_CODE"), Integer::New(MKDEXT_FENCED_CODE));
    target->Set(String::NewSymbol("EXT_LAX_HTML_BLOCKS"), Integer::New(MKDEXT_LAX_HTML_BLOCKS));
    target->Set(String::NewSymbol("EXT_NO_INTRA_EMPHASIS"), Integer::New(MKDEXT_NO_INTRA_EMPHASIS));
    target->Set(String::NewSymbol("EXT_SPACE_HEADERS"), Integer::New(MKDEXT_SPACE_HEADERS));
    target->Set(String::NewSymbol("EXT_STRIKETHROUGH"), Integer::New(MKDEXT_STRIKETHROUGH));
    target->Set(String::NewSymbol("EXT_TABLES"), Integer::New(MKDEXT_TABLES));

    //TODO: html renderer flags

    //Renderer class
    Local<FunctionTemplate> rendL = FunctionTemplate::New(&Renderer::newInstance);
    Persistent<FunctionTemplate> rend = Persistent<FunctionTemplate>::New(rendL);
    rend->InstanceTemplate()->SetInternalFieldCount(1);
    rend->SetClassName(String::NewSymbol("Renderer"));

    target->Set(String::NewSymbol("Renderer"), rend->GetFunction());

    //Standard HTML renderer class
    Local<FunctionTemplate> htmlrendL = FunctionTemplate::New(&HtmlRenderer::newInstance);
    Persistent<FunctionTemplate> htmlrend = Persistent<FunctionTemplate>::New(htmlrendL);
    htmlrend->InstanceTemplate()->SetInternalFieldCount(1);
    htmlrend->Inherit(rend);
    htmlrend->SetClassName(String::NewSymbol("HtmlRenderer"));

    target->Set(String::NewSymbol("HtmlRenderer"), htmlrend->GetFunction());
  }
  NODE_MODULE(robotskirt, init)
}
<commit_msg>Organize, cleanup original code; add some utilities<commit_after>#include <v8.h>
#include <node.h>
#include <node_buffer.h>

#include <string>

#include "markdownWrapper.hpp"

using namespace std;

using namespace node;
using namespace v8;

using namespace mkd;

#define OUTPUT_UNIT 64

////////////////////////////////////////////////////////////////////////////////
// UTILITIES to ease wrapping and interfacing with V8
////////////////////////////////////////////////////////////////////////////////

// Credit: @samcday
// http://sambro.is-super-awesome.com/2011/03/03/creating-a-proper-buffer-in-a-node-c-addon/
#define MAKE_FAST_BUFFER(NG_SLOW_BUFFER, NG_FAST_BUFFER)      \
  Local<Function> NG_JS_BUFFER = Local<Function>::Cast(       \
    Context::GetCurrent()->Global()->Get(                     \
      String::New("Buffer")));                                \
                                                              \
  Handle<Value> NG_JS_ARGS[3] = {                             \
    NG_SLOW_BUFFER->handle_,                                  \
    Integer::New(Buffer::Length(NG_SLOW_BUFFER)),             \
    Integer::New(0)                                           \
  };                                                          \
                                                              \
  NG_FAST_BUFFER = NG_JS_BUFFER->NewInstance(3, NG_JS_ARGS);

// V8 exception wrapping

#define V8_WRAP_START()                                                        \
  HandleScope scope;                                                           \
  try {

#define V8_WRAP_END()                                                          \
  } catch (Handle<Value> err) {                                                \
    return ThrowException(err);                                                \
  } catch (exception err) {                                                    \
    return ThrowException(Exception::Error(String::New(err.what())));          \
  }

#define V8_WRAP_END_NR()                                                       \
  } catch (Handle<Value> err) {                                                \
    ThrowException(err);                                                       \
  } catch (exception err) {                                                    \
    ThrowException(Exception::Error(String::New(err.what())));                 \
  }

// Dealing with V8 persistent handles

template <class T> void ClearPersistent(Persistent<T>& handle) {
    if (handle.IsEmpty()) return;
    handle.Dispose();
    handle.Clear();
}

template <class T> void SetPersistent(Persistent<T>& handle, Handle<T> value) {
    ClearPersistent<T>(handle);
    if (value.IsEmpty()) return;
    handle = Persistent<T>::New(value);
}

////////////////////////////////////////////////////////////////////////////////
// MODULE DECLARATION
////////////////////////////////////////////////////////////////////////////////

extern "C" {
  void init (Handle<Object> target) {
    HandleScope scope;

    //Static functions & properties
    target->Set(String::NewSymbol("version"), String::New("1.0.0"));
    NODE_SET_METHOD(target, "markdown", markdown);
    NODE_SET_METHOD(target, "markdownSync", markdownSync);

    //Extension constants
    target->Set(String::NewSymbol("EXT_AUTOLINK"), Integer::New(MKDEXT_AUTOLINK));
    target->Set(String::NewSymbol("EXT_FENCED_CODE"), Integer::New(MKDEXT_FENCED_CODE));
    target->Set(String::NewSymbol("EXT_LAX_HTML_BLOCKS"), Integer::New(MKDEXT_LAX_HTML_BLOCKS));
    target->Set(String::NewSymbol("EXT_NO_INTRA_EMPHASIS"), Integer::New(MKDEXT_NO_INTRA_EMPHASIS));
    target->Set(String::NewSymbol("EXT_SPACE_HEADERS"), Integer::New(MKDEXT_SPACE_HEADERS));
    target->Set(String::NewSymbol("EXT_STRIKETHROUGH"), Integer::New(MKDEXT_STRIKETHROUGH));
    target->Set(String::NewSymbol("EXT_TABLES"), Integer::New(MKDEXT_TABLES));

    //TODO: html renderer flags

    //TODO
  }
  NODE_MODULE(robotskirt, init)
}
<|endoftext|>


<commit_before>/// @file Environment.cpp
/// @brief Manages environment-related reX-logic, e.g. world time and lightning.
/// For conditions of distribution and use, see copyright notice in license.txt

#include "StableHeaders.h"
#include "EnvironmentModule.h"
#include "Environment.h"
#include <EC_OgreEnvironment.h>
#include <SceneManager.h>
#include <NetworkEvents.h>

#include <QVector>

namespace Environment
{

Environment::Environment(EnvironmentModule *owner) :
    owner_(owner),
    activeEnvEntity_(Scene::EntityWeakPtr()),
    time_override_(false),
    usecSinceStart_(0),
    secPerDay_(0),
    secPerYear_(0),
    sunDirection_(RexTypes::Vector3()),
    sunPhase_(0.0),
    sunAngVelocity_(RexTypes::Vector3())
{}

Environment::~Environment()
{
    // Does not own.
    activeEnvEntity_.reset();
    owner_ = 0;

}

OgreRenderer::EC_OgreEnvironment* Environment::GetEnvironmentComponent()
{
    if (activeEnvEntity_.expired())
        return 0;

    OgreRenderer::EC_OgreEnvironment* ec = activeEnvEntity_.lock()->GetComponent<OgreRenderer::EC_OgreEnvironment>().get();
    return ec;
}

Scene::EntityWeakPtr Environment::GetEnvironmentEntity()
{
    return activeEnvEntity_;
}

void Environment::CreateEnvironment()
{
    if ( !activeEnvEntity_.expired())
        return;

    Scene::ScenePtr active_scene = owner_->GetFramework()->GetDefaultWorldScene();
    Scene::EntityPtr entity = active_scene->CreateEntity(active_scene->GetNextFreeId());
    entity->AddEntityComponent(owner_->GetFramework()->GetComponentManager()->CreateComponent("EC_OgreEnvironment"));
    activeEnvEntity_ = entity;
}

bool Environment::HandleSimulatorViewerTimeMessage(ProtocolUtilities::NetworkEventInboundData *data)
{
    ProtocolUtilities::NetInMessage &msg = *data->message;
    msg.ResetReading();

    ///\ secPerDay,secPerYear, sunPhase seems to be zero, at least with 0.4 server
    usecSinceStart_ = (time_t)msg.ReadU64();
    secPerDay_ = msg.ReadU32();
    secPerYear_ = msg.ReadU32();
    sunDirection_ = msg.ReadVector3();
    sunPhase_ = msg.ReadF32();
    sunAngVelocity_ = msg.ReadVector3();

    // Calculate time of day from sun phase, which seems the most reliable way to do it
    float dayphase;

    if (sunPhase_ < 1.25 * PI)
        dayphase = (sunPhase_ / PI + 1.0) * 0.33333333;
    else
        dayphase = -0.5 + (sunPhase_ / PI);

    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return false;

    // Update the sunlight direction and angle velocity.
    ///\note Not needed anymore as we use Caelum now.
    //OgreRenderer::EC_OgreEnvironment &env = *checked_static_cast<OgreRenderer::EC_OgreEnvironment*>
    //    (component.get());
    //env.SetSunDirection(-sunDirection_);

    if (!time_override_)
    {
        env->SetTime(dayphase);
    }

    return false;
}

void Environment::SetWaterFog(float fogStart, float fogEnd, const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetWaterFogStart(fogStart);
    env->SetWaterFogEnd(fogEnd);
    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetWaterFogColor(fogColour);
    emit WaterFogAdjusted(fogStart, fogEnd, color);
}

void Environment::SetGroundFog(float fogStart, float fogEnd, const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetGroundFogStart(fogStart);
    env->SetGroundFogEnd(fogEnd);
    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetGroundFogColor(fogColour);
    emit GroundFogAdjusted(fogStart, fogEnd, color);
}

void Environment::SetFogColorOverride(bool enabled)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;
    env->SetFogColorOverride(enabled);
}

bool Environment::GetFogColorOverride()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return false;

    return env->GetFogColorOverride();
}

QVector<float> Environment::GetFogGroundColor()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>();

    Ogre::ColourValue color = env->GetGroundFogColor();
    QVector<float> vec;
    vec<<color[0]<<color[1]<<color[2];
    return vec;
}

QVector<float> Environment::GetFogWaterColor()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>();

    Ogre::ColourValue color = env->GetWaterFogColor();
    QVector<float> vec;
    vec<<color[0]<<color[1]<<color[2];
    return vec;
}

void Environment::Update(f64 frametime)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->UpdateVisualEffects(frametime);
}

bool Environment::IsCaelum()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return false;

    return env->IsCaelumUsed();
}

void Environment::SetGroundFogColor(const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetGroundFogColor(fogColour);
}

void Environment::SetWaterFogColor(const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetWaterFogColor(fogColour);
}

void Environment::SetGroundFogDistance(float fogStart, float fogEnd)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetGroundFogStart(fogStart);
    env->SetGroundFogEnd(fogEnd);
}

void Environment::SetWaterFogDistance(float fogStart, float fogEnd)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetWaterFogStart(fogStart);
    env->SetWaterFogEnd(fogEnd);
}

float Environment::GetWaterFogStartDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;

    return env->GetWaterFogStart();
}

float Environment::GetWaterFogEndDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;
    return env->GetWaterFogEnd();
}

float Environment::GetGroundFogStartDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;

    return env->GetGroundFogStart();
}

float Environment::GetGroundFogEndDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;

    return env->GetGroundFogEnd();
}

void Environment::SetSunDirection(const QVector<float>& vector)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetSunDirection(Vector3df(vector[0], vector[1], vector[2]));
}

QVector<float> Environment::GetSunDirection()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>(3);

    Vector3df vec = env->GetSunDirection();
    QVector<float> vector;
    vector.append(vec.x), vector.append(vec.y), vector.append(vec.z);
    return vector;
}

void Environment::SetSunColor(const QVector<float>& vector)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    if ( vector.size() == 4)
    {
        Color color(vector[0], vector[1], vector[2], vector[3]);
        env->SetSunColor(color);
    }
    else if ( vector.size() == 3 )
    {
        Color color(vector[0], vector[1], vector[2], 1.0);
        env->SetSunColor(color);
    }

}

QVector<float> Environment::GetSunColor()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>(4);

    Color color = env->GetSunColor();
    QVector<float> vec(4);
    vec[0] = color.r, vec[1] = color.g, vec[2] = color.b, vec[3] = color.a;
    return vec;
}

QVector<float> Environment::GetAmbientLight()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>(3);

   Color color = env->GetAmbientLightColor();
   QVector<float> vec(3);
   vec[0] = color.r, vec[1] = color.g, vec[2] = color.b;
   return vec;
}

void Environment::SetAmbientLight(const QVector<float>& vector)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetAmbientLightColor(Color(vector[0], vector[1], vector[2]));
}

}
<commit_msg>Check which assures that you cannot set zero direction vector to Sun light. <commit_after>/// @file Environment.cpp
/// @brief Manages environment-related reX-logic, e.g. world time and lightning.
/// For conditions of distribution and use, see copyright notice in license.txt

#include "StableHeaders.h"
#include "EnvironmentModule.h"
#include "Environment.h"
#include <EC_OgreEnvironment.h>
#include <SceneManager.h>
#include <NetworkEvents.h>

#include <QVector>

namespace Environment
{

Environment::Environment(EnvironmentModule *owner) :
    owner_(owner),
    activeEnvEntity_(Scene::EntityWeakPtr()),
    time_override_(false),
    usecSinceStart_(0),
    secPerDay_(0),
    secPerYear_(0),
    sunDirection_(RexTypes::Vector3()),
    sunPhase_(0.0),
    sunAngVelocity_(RexTypes::Vector3())
{}

Environment::~Environment()
{
    // Does not own.
    activeEnvEntity_.reset();
    owner_ = 0;

}

OgreRenderer::EC_OgreEnvironment* Environment::GetEnvironmentComponent()
{
    if (activeEnvEntity_.expired())
        return 0;

    OgreRenderer::EC_OgreEnvironment* ec = activeEnvEntity_.lock()->GetComponent<OgreRenderer::EC_OgreEnvironment>().get();
    return ec;
}

Scene::EntityWeakPtr Environment::GetEnvironmentEntity()
{
    return activeEnvEntity_;
}

void Environment::CreateEnvironment()
{
    if ( !activeEnvEntity_.expired())
        return;

    Scene::ScenePtr active_scene = owner_->GetFramework()->GetDefaultWorldScene();
    Scene::EntityPtr entity = active_scene->CreateEntity(active_scene->GetNextFreeId());
    entity->AddEntityComponent(owner_->GetFramework()->GetComponentManager()->CreateComponent("EC_OgreEnvironment"));
    activeEnvEntity_ = entity;
}

bool Environment::HandleSimulatorViewerTimeMessage(ProtocolUtilities::NetworkEventInboundData *data)
{
    ProtocolUtilities::NetInMessage &msg = *data->message;
    msg.ResetReading();

    ///\ secPerDay,secPerYear, sunPhase seems to be zero, at least with 0.4 server
    usecSinceStart_ = (time_t)msg.ReadU64();
    secPerDay_ = msg.ReadU32();
    secPerYear_ = msg.ReadU32();
    sunDirection_ = msg.ReadVector3();
    sunPhase_ = msg.ReadF32();
    sunAngVelocity_ = msg.ReadVector3();

    // Calculate time of day from sun phase, which seems the most reliable way to do it
    float dayphase;

    if (sunPhase_ < 1.25 * PI)
        dayphase = (sunPhase_ / PI + 1.0) * 0.33333333;
    else
        dayphase = -0.5 + (sunPhase_ / PI);

    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return false;

    // Update the sunlight direction and angle velocity.
    ///\note Not needed anymore as we use Caelum now.
    //OgreRenderer::EC_OgreEnvironment &env = *checked_static_cast<OgreRenderer::EC_OgreEnvironment*>
    //    (component.get());
    //env.SetSunDirection(-sunDirection_);

    if (!time_override_)
    {
        env->SetTime(dayphase);
    }

    return false;
}

void Environment::SetWaterFog(float fogStart, float fogEnd, const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetWaterFogStart(fogStart);
    env->SetWaterFogEnd(fogEnd);
    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetWaterFogColor(fogColour);
    emit WaterFogAdjusted(fogStart, fogEnd, color);
}

void Environment::SetGroundFog(float fogStart, float fogEnd, const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetGroundFogStart(fogStart);
    env->SetGroundFogEnd(fogEnd);
    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetGroundFogColor(fogColour);
    emit GroundFogAdjusted(fogStart, fogEnd, color);
}

void Environment::SetFogColorOverride(bool enabled)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;
    env->SetFogColorOverride(enabled);
}

bool Environment::GetFogColorOverride()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return false;

    return env->GetFogColorOverride();
}

QVector<float> Environment::GetFogGroundColor()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>();

    Ogre::ColourValue color = env->GetGroundFogColor();
    QVector<float> vec;
    vec<<color[0]<<color[1]<<color[2];
    return vec;
}

QVector<float> Environment::GetFogWaterColor()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>();

    Ogre::ColourValue color = env->GetWaterFogColor();
    QVector<float> vec;
    vec<<color[0]<<color[1]<<color[2];
    return vec;
}

void Environment::Update(f64 frametime)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->UpdateVisualEffects(frametime);
}

bool Environment::IsCaelum()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return false;

    return env->IsCaelumUsed();
}

void Environment::SetGroundFogColor(const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetGroundFogColor(fogColour);
}

void Environment::SetWaterFogColor(const QVector<float>& color)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    Ogre::ColourValue fogColour(color[0], color[1], color[2]);
    env->SetWaterFogColor(fogColour);
}

void Environment::SetGroundFogDistance(float fogStart, float fogEnd)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetGroundFogStart(fogStart);
    env->SetGroundFogEnd(fogEnd);
}

void Environment::SetWaterFogDistance(float fogStart, float fogEnd)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetWaterFogStart(fogStart);
    env->SetWaterFogEnd(fogEnd);
}

float Environment::GetWaterFogStartDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;

    return env->GetWaterFogStart();
}

float Environment::GetWaterFogEndDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;
    return env->GetWaterFogEnd();
}

float Environment::GetGroundFogStartDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;

    return env->GetGroundFogStart();
}

float Environment::GetGroundFogEndDistance()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return 0.0;

    return env->GetGroundFogEnd();
}

void Environment::SetSunDirection(const QVector<float>& vector)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;
    // Assure that we do not given too near of zero vector values, a la HACK.
    float squaredLength = vector[0] * vector[0] + vector[1]* vector[1] + vector[2] * vector[2];
    // Length must be diffrent then zero, so we say that value must be higher then our tolerance.
    float tolerance = 0.001;

    if ( squaredLength > tolerance)
       env->SetSunDirection(Vector3df(vector[0], vector[1], vector[2]));
}

QVector<float> Environment::GetSunDirection()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>(3);

    Vector3df vec = env->GetSunDirection();
    QVector<float> vector;
    vector.append(vec.x), vector.append(vec.y), vector.append(vec.z);
    return vector;
}

void Environment::SetSunColor(const QVector<float>& vector)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    if ( vector.size() == 4)
    {
        Color color(vector[0], vector[1], vector[2], vector[3]);
        env->SetSunColor(color);
    }
    else if ( vector.size() == 3 )
    {
        Color color(vector[0], vector[1], vector[2], 1.0);
        env->SetSunColor(color);
    }

}

QVector<float> Environment::GetSunColor()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>(4);

    Color color = env->GetSunColor();
    QVector<float> vec(4);
    vec[0] = color.r, vec[1] = color.g, vec[2] = color.b, vec[3] = color.a;
    return vec;
}

QVector<float> Environment::GetAmbientLight()
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return QVector<float>(3);

   Color color = env->GetAmbientLightColor();
   QVector<float> vec(3);
   vec[0] = color.r, vec[1] = color.g, vec[2] = color.b;
   return vec;
}

void Environment::SetAmbientLight(const QVector<float>& vector)
{
    OgreRenderer::EC_OgreEnvironment* env = GetEnvironmentComponent();
    if (!env)
        return;

    env->SetAmbientLightColor(Color(vector[0], vector[1], vector[2]));
}

}
<|endoftext|>


<commit_before>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "QmitkRegisterClasses.h"
#include "QmitkRenderWindow.h"
#include "QmitkSliceWidget.h"

#include "mitkDataNodeFactory.h"
#include "mitkProperties.h"
#include "mitkRenderingManager.h"
#include "mitkStandaloneDataStorage.h"

#include <itksys/SystemTools.hxx>
#include <QApplication>
#include <QHBoxLayout>

//##Documentation
//## @brief Use several views to explore data
//##
//## As in Step2 and Step3, load one or more data sets (many image,
//## surface and other formats), but create 3 views on the data.
//## The QmitkRenderWindow is used for displaying a 3D view as in Step3,
//## but without volume-rendering.
//## Furthermore, we create two 2D views for slicing through the data.
//## We use the class QmitkSliceWidget, which is based on the class
//## QmitkRenderWindow, but additionally provides sliders
//## to slice through the data. We create two instances of
//## QmitkSliceWidget, one for axial and one for sagittal slicing.
//## The two slices are also shown at their correct position in 3D as
//## well as intersection-line, each in the other 2D view.
int main(int argc, char* argv[])
{
  QApplication qtapplication( argc, argv );

  if(argc<2)
  {
    fprintf( stderr, "Usage:   %s [filename1] [filename2] ...\n\n", itksys::SystemTools::GetFilenameName(argv[0]).c_str() );
    return 1;
  }

  // Register Qmitk-dependent global instances
  QmitkRegisterClasses();

  //*************************************************************************
  // Part I: Basic initialization
  //*************************************************************************

  // Create a DataStorage
  mitk::StandaloneDataStorage::Pointer ds = mitk::StandaloneDataStorage::New();

  //*************************************************************************
  // Part II: Create some data by reading files
  //*************************************************************************
  int i;
  for(i=1; i<argc; ++i)
  {
    // For testing
    if(strcmp(argv[i], "-testing")==0) continue;

    // Create a DataNodeFactory to read a data format supported
    // by the DataNodeFactory (many image formats, surface formats, etc.)
    mitk::DataNodeFactory::Pointer nodeReader=mitk::DataNodeFactory::New();
    const char * filename = argv[i];
    try
    {
      nodeReader->SetFileName(filename);
      nodeReader->Update();
      //*********************************************************************
      //Part III: Put the data into the datastorage
      //*********************************************************************

      // Since the DataNodeFactory directly creates a node,
      // use the datastorage to add the read node
      mitk::DataNode::Pointer node = nodeReader->GetOutput();
      ds->Add(node);
    }
    catch(...)
    {
      fprintf( stderr, "Could not open file %s \n\n", filename );
      exit(2);
    }
  }

  //*************************************************************************
  // Part IV: Create windows and pass the tree to it
  //*************************************************************************

  // Create toplevel widget with horizontal layout
  QWidget toplevelWidget;
  QHBoxLayout layout;
  layout.setSpacing(2);
  layout.setMargin(0);
  toplevelWidget.setLayout(&layout);

  //*************************************************************************
  // Part IVa: 3D view
  //*************************************************************************

  // Create a renderwindow
  QmitkRenderWindow renderWindow(&toplevelWidget);
  layout.addWidget(&renderWindow);

  // Tell the renderwindow which (part of) the datastorage to render
  renderWindow.GetRenderer()->SetDataStorage(ds);

  // Use it as a 3D view
  renderWindow.GetRenderer()->SetMapperID(mitk::BaseRenderer::Standard3D);

  // *******************************************************
  // ****************** START OF NEW PART ******************
  // *******************************************************

  //*************************************************************************
  // Part IVb: 2D view for slicing axially
  //*************************************************************************

  // Create QmitkSliceWidget, which is based on the class
  // QmitkRenderWindow, but additionally provides sliders
  QmitkSliceWidget view2(&toplevelWidget);
  layout.addWidget(&view2);
  view2.SetLevelWindowEnabled(true);
  // Tell the QmitkSliceWidget which (part of) the tree to render.
  // By default, it slices the data axially
  view2.SetDataStorage(ds);
  mitk::DataStorage::SetOfObjects::ConstPointer rs = ds->GetAll();
  view2.SetData(rs->Begin(),mitk::SliceNavigationController::Axial);
  // We want to see the position of the slice in 2D and the
  // slice itself in 3D: add it to the datastorage!
  ds->Add(view2.GetRenderer()->GetCurrentWorldGeometry2DNode());

  //*************************************************************************
  // Part IVc: 2D view for slicing sagitally
  //*************************************************************************

  // Create QmitkSliceWidget, which is based on the class
  // QmitkRenderWindow, but additionally provides sliders
  QmitkSliceWidget view3(&toplevelWidget);
  layout.addWidget(&view3);
  view3.SetDataStorage(ds);
  // Tell the QmitkSliceWidget which (part of) the datastorage to render
  // and to slice sagitally
  view3.SetData(rs->Begin(), mitk::SliceNavigationController::Sagittal);
  // We want to see the position of the slice in 2D and the
  // slice itself in 3D: add it to the datastorage!
  ds->Add(view3.GetRenderer()->GetCurrentWorldGeometry2DNode());

  // *******************************************************
  // ******************* END OF NEW PART *******************
  // *******************************************************

  //*************************************************************************
  // Part V: Qt-specific initialization
  //*************************************************************************
  toplevelWidget.show();

  // for testing
  #include "QtTesting.h"
  if(strcmp(argv[argc-1], "-testing")!=0)
    return qtapplication.exec();
  else
    return QtTesting();
}

/**
\example Step4.cpp
*/
<commit_msg>Use node predicate to get the correct node from the data storage.<commit_after>/*===================================================================

The Medical Imaging Interaction Toolkit (MITK)

Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.

This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.

See LICENSE.txt or http://www.mitk.org for details.

===================================================================*/

#include "QmitkRegisterClasses.h"
#include "QmitkRenderWindow.h"
#include "QmitkSliceWidget.h"

#include "mitkDataNodeFactory.h"
#include "mitkProperties.h"
#include "mitkRenderingManager.h"
#include "mitkStandaloneDataStorage.h"
#include "mitkNodePredicateDataType.h"

#include <itksys/SystemTools.hxx>
#include <QApplication>
#include <QHBoxLayout>

//##Documentation
//## @brief Use several views to explore data
//##
//## As in Step2 and Step3, load one or more data sets (many image,
//## surface and other formats), but create 3 views on the data.
//## The QmitkRenderWindow is used for displaying a 3D view as in Step3,
//## but without volume-rendering.
//## Furthermore, we create two 2D views for slicing through the data.
//## We use the class QmitkSliceWidget, which is based on the class
//## QmitkRenderWindow, but additionally provides sliders
//## to slice through the data. We create two instances of
//## QmitkSliceWidget, one for axial and one for sagittal slicing.
//## The two slices are also shown at their correct position in 3D as
//## well as intersection-line, each in the other 2D view.
int main(int argc, char* argv[])
{
  QApplication qtapplication( argc, argv );

  if(argc<2)
  {
    fprintf( stderr, "Usage:   %s [filename1] [filename2] ...\n\n", itksys::SystemTools::GetFilenameName(argv[0]).c_str() );
    return 1;
  }

  // Register Qmitk-dependent global instances
  QmitkRegisterClasses();

  //*************************************************************************
  // Part I: Basic initialization
  //*************************************************************************

  // Create a DataStorage
  mitk::StandaloneDataStorage::Pointer ds = mitk::StandaloneDataStorage::New();

  //*************************************************************************
  // Part II: Create some data by reading files
  //*************************************************************************
  int i;
  for(i=1; i<argc; ++i)
  {
    // For testing
    if(strcmp(argv[i], "-testing")==0) continue;

    // Create a DataNodeFactory to read a data format supported
    // by the DataNodeFactory (many image formats, surface formats, etc.)
    mitk::DataNodeFactory::Pointer nodeReader=mitk::DataNodeFactory::New();
    const char * filename = argv[i];
    try
    {
      nodeReader->SetFileName(filename);
      nodeReader->Update();
      //*********************************************************************
      //Part III: Put the data into the datastorage
      //*********************************************************************

      // Since the DataNodeFactory directly creates a node,
      // use the datastorage to add the read node
      mitk::DataNode::Pointer node = nodeReader->GetOutput();
      ds->Add(node);
    }
    catch(...)
    {
      fprintf( stderr, "Could not open file %s \n\n", filename );
      exit(2);
    }
  }

  //*************************************************************************
  // Part IV: Create windows and pass the tree to it
  //*************************************************************************

  // Create toplevel widget with horizontal layout
  QWidget toplevelWidget;
  QHBoxLayout layout;
  layout.setSpacing(2);
  layout.setMargin(0);
  toplevelWidget.setLayout(&layout);

  //*************************************************************************
  // Part IVa: 3D view
  //*************************************************************************

  // Create a renderwindow
  QmitkRenderWindow renderWindow(&toplevelWidget);
  layout.addWidget(&renderWindow);

  // Tell the renderwindow which (part of) the datastorage to render
  renderWindow.GetRenderer()->SetDataStorage(ds);

  // Use it as a 3D view
  renderWindow.GetRenderer()->SetMapperID(mitk::BaseRenderer::Standard3D);

  // *******************************************************
  // ****************** START OF NEW PART ******************
  // *******************************************************

  //*************************************************************************
  // Part IVb: 2D view for slicing axially
  //*************************************************************************

  // Create QmitkSliceWidget, which is based on the class
  // QmitkRenderWindow, but additionally provides sliders
  QmitkSliceWidget view2(&toplevelWidget);
  layout.addWidget(&view2);
  view2.SetLevelWindowEnabled(true);
  // Tell the QmitkSliceWidget which (part of) the tree to render.
  // By default, it slices the data axially
  view2.SetDataStorage(ds);

  // Get the image from the data storage. A predicate (mitk::NodePredicateBase)
  // is used to get only nodes of the type mitk::Image.
  mitk::DataStorage::SetOfObjects::ConstPointer rs =
      ds->GetSubset(mitk::TNodePredicateDataType<mitk::Image>::New());

  view2.SetData(rs->Begin(),mitk::SliceNavigationController::Axial);
  // We want to see the position of the slice in 2D and the
  // slice itself in 3D: add it to the datastorage!
  ds->Add(view2.GetRenderer()->GetCurrentWorldGeometry2DNode());

  //*************************************************************************
  // Part IVc: 2D view for slicing sagitally
  //*************************************************************************

  // Create QmitkSliceWidget, which is based on the class
  // QmitkRenderWindow, but additionally provides sliders
  QmitkSliceWidget view3(&toplevelWidget);
  layout.addWidget(&view3);
  view3.SetDataStorage(ds);
  // Tell the QmitkSliceWidget which (part of) the datastorage to render
  // and to slice sagitally
  view3.SetData(rs->Begin(), mitk::SliceNavigationController::Sagittal);
  // We want to see the position of the slice in 2D and the
  // slice itself in 3D: add it to the datastorage!
  ds->Add(view3.GetRenderer()->GetCurrentWorldGeometry2DNode());

  // *******************************************************
  // ******************* END OF NEW PART *******************
  // *******************************************************

  //*************************************************************************
  // Part V: Qt-specific initialization
  //*************************************************************************
  toplevelWidget.show();

  // for testing
  #include "QtTesting.h"
  if(strcmp(argv[argc-1], "-testing")!=0)
    return qtapplication.exec();
  else
    return QtTesting();
}

/**
\example Step4.cpp
*/
<|endoftext|>


<commit_before>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// Test for non-capped cylinder, by Bram Stolk

#include <assert.h>
#include <unistd.h>
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#include "world_geom3.h" // this is our world mesh

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

#define BOX
#define CYL

// some constants

#define RADIUS 0.22	// wheel radius
#define WMASS 0.2	// wheel mass
#define WHEELW 0.2	// wheel width
#define BOXSZ 0.4	// box size

// dynamics and collision objects (chassis, 3 wheels, environment)

static dWorldID world;
static dSpaceID space;
#ifdef BOX
static dBodyID boxbody;
static dGeomID boxgeom;
#endif
#ifdef CYL
static dBodyID cylbody;
static dGeomID cylgeom;
#endif
static dJointGroupID contactgroup;
static dGeomID world_mesh;

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  assert(o1);
  assert(o2);

  if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
  {
    fprintf(stderr,"testing space %p %p\n", o1,o2);
    // colliding a space with something
    dSpaceCollide2(o1,o2,data,&nearCallback);
    // Note we do not want to test intersections within a space,
    // only between spaces.
    return;
  }

//  fprintf(stderr,"testing geoms %p %p\n", o1, o2);

  const int N = 32;
  dContact contact[N];
  int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
  if (n > 0)
  {
    for (int i=0; i<n; i++)
    {
      contact[i].surface.slip1 = 0.7;
      contact[i].surface.slip2 = 0.7;
      contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
      contact[i].surface.mu = 50.0; // was: dInfinity
      contact[i].surface.soft_erp = 0.96;
      contact[i].surface.soft_cfm = 0.04;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
		    dGeomGetBody(contact[i].geom.g1),
		    dGeomGetBody(contact[i].geom.g2));
    }
  }
}

// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {-8,-9,3};
  static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}

// called when a key pressed

static void command (int cmd)
{
  switch (cmd)
  {
    case ' ':
      break;
  }
}

// simulation loop

static void simLoop (int pause)
{
  int i;

  dSpaceCollide (space,0,&nearCallback);
  dWorldQuickStep (world,0.02); // assume 50 fps
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,1);
#ifdef BOX
  const dReal *BPos = dBodyGetPosition(boxbody);
  const dReal *BRot = dBodyGetRotation(boxbody);
  float bpos[3] = {BPos[0], BPos[1], BPos[2]};
  float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] };
  float sides[3] = {BOXSZ, BOXSZ, BOXSZ};
  dsDrawBox
  (
    bpos,
    brot,
    sides
  ); // single precision
#endif
#ifdef CYL
  const dReal *CPos = dBodyGetPosition(cylbody);
  const dReal *CRot = dBodyGetRotation(cylbody);
  float cpos[3] = {CPos[0], CPos[1], CPos[2]};
  float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
  dsDrawCylinder
  (
//    dBodyGetPosition(cylbody),
//    dBodyGetRotation(cylbody),
    cpos,
    crot,
    WHEELW,
    RADIUS
  ); // single precision
#endif

  // draw world trimesh
  dsSetColor(0.7,0.7,0.4);
  dsSetTexture (DS_NONE);

  const dReal* Pos = dGeomGetPosition(world_mesh);
  float pos[3] = { Pos[0], Pos[1], Pos[2] };

  const dReal* Rot = dGeomGetRotation(world_mesh);
  float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };

  int numi = sizeof(world_indices)  / sizeof(int);

  for (int i=0; i<numi/3; i++)
  {
    int i0 = world_indices[i*3+0];
    int i1 = world_indices[i*3+1];
    int i2 = world_indices[i*3+2];
    float *v0 = world_vertices+i0*3;
    float *v1 = world_vertices+i1*3;
    float *v2 = world_vertices+i2*3;
    dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
  }
}

int main (int argc, char **argv)
{
  int i;
  dMass m;
  dMatrix3 R;

  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.8);
  dWorldSetQuickStepNumIterations (world, 64);

  // Create a static world using a triangle mesh that we can collide with.
  int numv = sizeof(world_vertices)/sizeof(dReal);
  int numi = sizeof(world_indices)/ sizeof(int);
  printf("numv=%d, numi=%d\n", numv, numi);
  dTriMeshDataID Data = dGeomTriMeshDataCreate();

  // Super weird: comment out next printf, and run with single-precision:
  // collisions are not detected.
//  fprintf(stderr,"Building Single Precision Mesh\n");

  dGeomTriMeshDataBuildSingle
  (
    Data,
    world_vertices,
    3 * sizeof(float),
    numv,
    world_indices,
    numi,
    3 * sizeof(int)
  );

  world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
  dGeomSetPosition(world_mesh, 0, 0, 0.5);
  dRFromAxisAndAngle (R, 0,1,0, 0.0);
  dGeomSetRotation (world_mesh, R);

  float sx=-4, sy=-4, sz=2;
  dQuaternion q;
  dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
  boxbody = dBodyCreate (world);
  dBodySetPosition (boxbody,sx,sy+1.0,sz);
  dBodySetQuaternion (boxbody,q);
  dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
  dMassAdjust (&m, 1);
  dBodySetMass (boxbody,&m);
  boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
  dGeomSetBody (boxgeom,boxbody);
  dSpaceAdd (space, boxgeom);
#endif
#ifdef CYL
  cylbody = dBodyCreate (world);
  dBodySetQuaternion (cylbody,q);
  dMassSetSphere (&m,1,RADIUS);
  dMassAdjust (&m,WMASS);
  dBodySetMass (cylbody,&m);
  cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
  dGeomSetBody (cylgeom,cylbody);
  dBodySetPosition (cylbody, sx, sy, sz);
  dSpaceAdd (space, cylgeom);
#endif

  // run simulation
  dsSimulationLoop (argc,argv,704,576,&fn);

  dJointGroupEmpty (contactgroup);
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  // Causes segm violation? Why?
#ifdef CYL
//  dGeomDestroy (cylgeom);
#endif
#ifdef BOX
//  dGeomDestroy (boxgeom);
#endif
//  dGeomDestroy (world_mesh);

  return 0;
}

<commit_msg>Fixed win32 build, patch 1431678<commit_after>/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/

// Test for non-capped cylinder, by Bram Stolk

#include <assert.h>
#ifndef WIN32
#include <unistd.h>
#endif
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>

#include "world_geom3.h" // this is our world mesh

#ifdef _MSC_VER
#pragma warning(disable:4244 4305)  // for VC++, no precision loss complaints
#endif

#define BOX
#define CYL

// some constants

#define RADIUS 0.22	// wheel radius
#define WMASS 0.2	// wheel mass
#define WHEELW 0.2	// wheel width
#define BOXSZ 0.4	// box size

// dynamics and collision objects (chassis, 3 wheels, environment)

static dWorldID world;
static dSpaceID space;
#ifdef BOX
static dBodyID boxbody;
static dGeomID boxgeom;
#endif
#ifdef CYL
static dBodyID cylbody;
static dGeomID cylgeom;
#endif
static dJointGroupID contactgroup;
static dGeomID world_mesh;

// this is called by dSpaceCollide when two objects in space are
// potentially colliding.

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  assert(o1);
  assert(o2);

  if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
  {
    fprintf(stderr,"testing space %p %p\n", o1,o2);
    // colliding a space with something
    dSpaceCollide2(o1,o2,data,&nearCallback);
    // Note we do not want to test intersections within a space,
    // only between spaces.
    return;
  }

//  fprintf(stderr,"testing geoms %p %p\n", o1, o2);

  const int N = 32;
  dContact contact[N];
  int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
  if (n > 0)
  {
    for (int i=0; i<n; i++)
    {
      contact[i].surface.slip1 = 0.7;
      contact[i].surface.slip2 = 0.7;
      contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1 | dContactSlip1 | dContactSlip2;
      contact[i].surface.mu = 50.0; // was: dInfinity
      contact[i].surface.soft_erp = 0.96;
      contact[i].surface.soft_cfm = 0.04;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
		    dGeomGetBody(contact[i].geom.g1),
		    dGeomGetBody(contact[i].geom.g2));
    }
  }
}

// start simulation - set viewpoint

static void start()
{
  static float xyz[3] = {-8,-9,3};
  static float hpr[3] = {45.0000f,-27.5000f,0.0000f};
  dsSetViewpoint (xyz,hpr);
}

// called when a key pressed

static void command (int cmd)
{
  switch (cmd)
  {
    case ' ':
      break;
  }
}

// simulation loop

static void simLoop (int pause)
{
  dSpaceCollide (space,0,&nearCallback);
  dWorldQuickStep (world,0.02); // assume 50 fps
  dJointGroupEmpty (contactgroup);

  dsSetColor (1,1,1);
#ifdef BOX
  const dReal *BPos = dBodyGetPosition(boxbody);
  const dReal *BRot = dBodyGetRotation(boxbody);
  float bpos[3] = {BPos[0], BPos[1], BPos[2]};
  float brot[12] = { BRot[0], BRot[1], BRot[2], BRot[3], BRot[4], BRot[5], BRot[6], BRot[7], BRot[8], BRot[9], BRot[10], BRot[11] };
  float sides[3] = {BOXSZ, BOXSZ, BOXSZ};
  dsDrawBox
  (
    bpos,
    brot,
    sides
  ); // single precision
#endif
#ifdef CYL
  const dReal *CPos = dBodyGetPosition(cylbody);
  const dReal *CRot = dBodyGetRotation(cylbody);
  float cpos[3] = {CPos[0], CPos[1], CPos[2]};
  float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
  dsDrawCylinder
  (
//    dBodyGetPosition(cylbody),
//    dBodyGetRotation(cylbody),
    cpos,
    crot,
    WHEELW,
    RADIUS
  ); // single precision
#endif

  // draw world trimesh
  dsSetColor(0.7,0.7,0.4);
  dsSetTexture (DS_NONE);

  const dReal* Pos = dGeomGetPosition(world_mesh);
  float pos[3] = { Pos[0], Pos[1], Pos[2] };

  const dReal* Rot = dGeomGetRotation(world_mesh);
  float rot[12] = { Rot[0], Rot[1], Rot[2], Rot[3], Rot[4], Rot[5], Rot[6], Rot[7], Rot[8], Rot[9], Rot[10], Rot[11] };

  int numi = sizeof(world_indices)  / sizeof(int);

  for (int i=0; i<numi/3; i++)
  {
    int i0 = world_indices[i*3+0];
    int i1 = world_indices[i*3+1];
    int i2 = world_indices[i*3+2];
    float *v0 = world_vertices+i0*3;
    float *v1 = world_vertices+i1*3;
    float *v2 = world_vertices+i2*3;
    dsDrawTriangle(pos, rot, v0,v1,v2, true); // single precision draw
  }
}

int main (int argc, char **argv)
{
  int i;
  dMass m;
  dMatrix3 R;

  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = "../../drawstuff/textures";
  if(argc==2)
    {
        fn.path_to_textures = argv[1];
    }

  // create world
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.8);
  dWorldSetQuickStepNumIterations (world, 64);

  // Create a static world using a triangle mesh that we can collide with.
  int numv = sizeof(world_vertices)/sizeof(dReal);
  int numi = sizeof(world_indices)/ sizeof(int);
  printf("numv=%d, numi=%d\n", numv, numi);
  dTriMeshDataID Data = dGeomTriMeshDataCreate();

  // Super weird: comment out next printf, and run with single-precision:
  // collisions are not detected.
//  fprintf(stderr,"Building Single Precision Mesh\n");

  dGeomTriMeshDataBuildSingle
  (
    Data,
    world_vertices,
    3 * sizeof(float),
    numv,
    world_indices,
    numi,
    3 * sizeof(int)
  );

  world_mesh = dCreateTriMesh(space, Data, 0, 0, 0);
  dGeomSetPosition(world_mesh, 0, 0, 0.5);
  dRFromAxisAndAngle (R, 0,1,0, 0.0);
  dGeomSetRotation (world_mesh, R);

  float sx=-4, sy=-4, sz=2;
  dQuaternion q;
  dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
  boxbody = dBodyCreate (world);
  dBodySetPosition (boxbody,sx,sy+1.0,sz);
  dBodySetQuaternion (boxbody,q);
  dMassSetBox (&m,1, BOXSZ, BOXSZ, BOXSZ);
  dMassAdjust (&m, 1);
  dBodySetMass (boxbody,&m);
  boxgeom = dCreateBox (0, BOXSZ, BOXSZ, BOXSZ);
  dGeomSetBody (boxgeom,boxbody);
  dSpaceAdd (space, boxgeom);
#endif
#ifdef CYL
  cylbody = dBodyCreate (world);
  dBodySetQuaternion (cylbody,q);
  dMassSetSphere (&m,1,RADIUS);
  dMassAdjust (&m,WMASS);
  dBodySetMass (cylbody,&m);
  cylgeom = dCreateCylinder(0, RADIUS, WHEELW);
  dGeomSetBody (cylgeom,cylbody);
  dBodySetPosition (cylbody, sx, sy, sz);
  dSpaceAdd (space, cylgeom);
#endif

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupEmpty (contactgroup);
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);

  // Causes segm violation? Why?
#ifdef CYL
//  dGeomDestroy (cylgeom);
#endif
#ifdef BOX
//  dGeomDestroy (boxgeom);
#endif
//  dGeomDestroy (world_mesh);

  return 0;
}

<|endoftext|>