ColumnVector MagneticModel::ExpectedValueGet() const {
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
y_(1) = (qw*qw+qx*qx-qy*qy-qz*qz) * magnetic_field_(1) + (2.0*qx*qy+2.0*qw*qz) * magnetic_field_(2) + (2.0*qx*qz-2.0*qw*qy) * magnetic_field_(3);
y_(2) = (2.0*qx*qy-2.0*qw*qz) * magnetic_field_(1) + (qw*qw-qx*qx+qy*qy-qz*qz) * magnetic_field_(2) + (2.0*qy*qz+2.0*qw*qx) * magnetic_field_(3);
y_(3) = (2.0*qx*qz+2.0*qw*qy) * magnetic_field_(1) + (2.0*qy*qz-2.0*qw*qx) * magnetic_field_(2) + (qw*qw-qx*qx-qy*qy+qz*qz) * magnetic_field_(3);
return y_;
}
Point divide_interval(Interval tmp, Fraction K)
{
Point tmp_point;
Fraction S_lg_X = tmp.getS().getX();
Fraction S_lg_Y = tmp.getS().getY();
Fraction E_lg_X = tmp.getE().getX();
Fraction E_lg_Y = tmp.getE().getY();
Fraction rvec_X = E_lg_X - S_lg_X;
Fraction rvec_Y = E_lg_Y - S_lg_Y;
if(K.lg_u_() + rvec_X.lg_u_() + S_lg_X.lg_l_() > 17 ||
K.lg_l_() + rvec_X.lg_l_() + S_lg_X.lg_u_() > 17 ||
K.lg_u_() + rvec_Y.lg_u_() + S_lg_Y.lg_l_() > 17 ||
K.lg_l_() + rvec_Y.lg_l_() + S_lg_Y.lg_u_() > 17 ||
K.lg_l_() + rvec_X.lg_l_() + S_lg_X.lg_l_() > 18 ||
K.lg_l_() + rvec_Y.lg_l_() + S_lg_Y.lg_l_() > 18)
{
double tmp_x = S_lg_X.numerical() + K.numerical() * (E_lg_X.numerical() - S_lg_X.numerical());
double tmp_y = S_lg_Y.numerical() + K.numerical() * (E_lg_Y.numerical() - S_lg_Y.numerical());
Fraction x_(tmp_x, 17);
Fraction y_(tmp_y, 17);
return tmp_point.upload(x_, y_);
}
else
{
Fraction x = S_lg_X + K * rvec_X;
Fraction y = S_lg_Y + K * rvec_Y;
return tmp_point.upload(x, y);
}
}
static void docx_rect(double x0, double y0, double x1, double y1,
const pGEcontext gc, pDevDesc dd) {
DOCX_dev *docx_obj = (DOCX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(2);
Rcpp::NumericVector y_(2);
x_[0] = x0;
y_[0] = y0;
x_[1] = x1;
y_[1] = y1;
docx_obj->clp->set_data(x_, y_);
docx_obj->clp->clip_polygon();
Rcpp::NumericVector x__ = docx_obj->clp->get_x();
Rcpp::NumericVector y__ = docx_obj->clp->get_y();
xfrm xfrm_(x__, y__);
line_style line_style_(gc->lwd, gc->col, gc->lty, gc->ljoin, gc->lend);
a_color fill_( gc->fill );
fputs("<wps:wsp>", docx_obj->file);
write_nv_pr_docx(dd, "rc");
fputs("<wps:spPr>", docx_obj->file);
fprintf(docx_obj->file, "%s", xfrm_.xml().c_str());
fprintf(docx_obj->file,"%s", a_prstgeom::a_tag("rect").c_str());
if( fill_.is_visible() > 0 )
fprintf(docx_obj->file, "%s", fill_.solid_fill().c_str());
fprintf(docx_obj->file, "%s", line_style_.a_tag().c_str());
fputs("</wps:spPr>", docx_obj->file);
fprintf(docx_obj->file, "%s",empty_body_text::wps_tag().c_str());
fputs("</wps:wsp>", docx_obj->file);
}
// Perform one iteration of the Kalman Filter for a CARMA(p,q) process to update it
void KalmanFilterp::Update() {
// First compute the Kalman Gain
kalman_gain_ = PredictionVar_ * rotated_ma_coefs_.t() / var(current_index_-1);
// Now update the state vector
state_vector_ += kalman_gain_ * innovation_;
// Update the state one-step prediction error variance
PredictionVar_ -= var(current_index_-1) * (kalman_gain_ * kalman_gain_.t());
// Predict the next state
rho_ = arma::exp(omega_ * dt_(current_index_-1));
state_vector_ = rho_ % state_vector_;
// Update the predicted state variance matrix
PredictionVar_ = (rho_ * rho_.t()) % (PredictionVar_ - StateVar_) + StateVar_;
// Now predict the observation and its variance.
mean(current_index_) = std::real( arma::as_scalar(rotated_ma_coefs_ * state_vector_) );
var(current_index_) = std::real( arma::as_scalar(rotated_ma_coefs_ * PredictionVar_ * rotated_ma_coefs_.t()) );
var(current_index_) += yerr_(current_index_) * yerr_(current_index_); // Add in measurement error contribution
// Finally, update the innovation
innovation_ = y_(current_index_) - mean(current_index_);
current_index_++;
}
main()
{
extern long y_();
long i;
i = y_();
}
void
arrangement::keep_arc(Arrangement_2::Edge_iterator &e, Arrangement_2 ©, Walk_pl &walk_pl)
{
e->set_data("none");
Conic_point_2 p;
// if it is a segment
if (e->curve().orientation() == CGAL::COLLINEAR)
{
Conic_point_2 source = e->curve().source();
Conic_point_2 target = e->curve().target();
double x = CGAL::to_double((target.x() + source.x()) /2);
double y = CGAL::to_double((target.y() + source.y()) /2);
Rational x_(x);
Rational y_(y);
p = Conic_point_2(x_,y_);
}
else // if it is an arc
{
int n = 2;
approximated_point_2* points = new approximated_point_2[n + 1];
e->curve().polyline_approximation(n, points); // there is 3 points
p = Conic_point_2(Rational(points[1].first),Rational(points[1].second));
}
Arrangement_2::Vertex_handle v = insert_point(copy, p, walk_pl);
try
{
if (v->face()->data() != 1)
nonCriticalRegions.remove_edge(e, false, false);
copy.remove_isolated_vertex(v);
}
catch (const std::exception exn) {}
}
// Pass interpolation datapoints as vectors.
void spline::vectors (qucs::vector y, qucs::vector t) {
int i = t.getSize ();
assert (y.getSize () == i && i >= 3);
// create local copy of f(x)
realloc (i);
for (i = 0; i <= n; i++) {
f0[i] = real (y_(i)); x[i] = real (t_(i));
}
}
// Pass interpolation datapoints as tvectors.
void spline::vectors (tvector<nr_double_t> y, tvector<nr_double_t> t) {
int i = t.size ();
assert (y.size () == i && i >= 3);
// create local copy of f(x)
realloc (i);
for (i = 0; i <= n; i++) {
f0[i] = y_(i); x[i] = t_(i);
}
}
ColumnVector HeadingModel::ExpectedValueGet() const {
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
y_(1) = atan2(2*(qx*qy + qw*qz), qw*qw + qx*qx - qy*qy - qz*qz);
return y_;
}
// Reset the Kalman Filter for a CARMA(p,q) process
void KalmanFilterp::Reset() {
// Initialize the matrix of Eigenvectors. We will work with the state vector
// in the space spanned by the Eigenvectors because in this space the state
// transition matrix is diagonal, so the calculation of the matrix exponential
// is fast.
arma::cx_mat EigenMat(p_,p_);
EigenMat.row(0) = arma::ones<arma::cx_rowvec>(p_);
EigenMat.row(1) = omega_.st();
for (int i=2; i<p_; i++) {
EigenMat.row(i) = strans(arma::pow(omega_, i));
}
// Input vector under original state space representation
arma::cx_vec Rvector = arma::zeros<arma::cx_vec>(p_);
Rvector(p_-1) = 1.0;
// Transform the input vector to the rotated state space representation.
// The notation R and J comes from Belcher et al. (1994).
arma::cx_vec Jvector(p_);
Jvector = arma::solve(EigenMat, Rvector);
// Transform the moving average coefficients to the space spanned by EigenMat.
rotated_ma_coefs_ = ma_coefs_ * EigenMat;
// Calculate the stationary covariance matrix of the state vector.
for (int i=0; i<p_; i++) {
for (int j=i; j<p_; j++) {
// Only fill in upper triangle of StateVar because of symmetry
StateVar_(i,j) = -sigsqr_ * Jvector(i) * std::conj(Jvector(j)) /
(omega_(i) + std::conj(omega_(j)));
}
}
StateVar_ = arma::symmatu(StateVar_); // StateVar is symmetric
PredictionVar_ = StateVar_; // One-step state prediction error
state_vector_.zeros(); // Initial state is set to zero
// Initialize the Kalman mean and variance. These are the forecasted value
// for the measured time series values and its variance, conditional on the
// previous measurements
mean(0) = 0.0;
var(0) = std::real( arma::as_scalar(rotated_ma_coefs_ * StateVar_ * rotated_ma_coefs_.t()) );
var(0) += yerr_(0) * yerr_(0); // Add in measurement error contribution
innovation_ = y_(0); // The innovation
current_index_ = 1;
}
GPSModel::MeasurementVector const& GPS::getValue(const GPSUpdate &update) {
if (!reference_) {
y_(1) = y_(2) = y_(3) = y_(4) = 0.0/0.0;
return y_;
}
double north = reference_->radius_north * (update.latitude - reference_->latitude);
double east = reference_->radius_east * (update.longitude - reference_->longitude);
y_(1) = north * reference_->cos_heading + east * reference_->sin_heading;
y_(2) = north * reference_->sin_heading - east * reference_->cos_heading;
y_(3) = update.velocity_north * reference_->cos_heading + update.velocity_east * reference_->sin_heading;
y_(4) = update.velocity_north * reference_->sin_heading - update.velocity_east * reference_->cos_heading;
last_ = update;
return y_;
}
void Foam::inverseDistanceDiffusivity::correct()
{
volScalarField y_
(
IOobject
(
"y",
mesh().time().timeName(),
mesh()
),
mesh(),
dimless,
zeroGradientFvPatchScalarField::typeName
);
y_.internalField() = y();
y_.correctBoundaryConditions();
faceDiffusivity_ = 1.0/fvc::interpolate(y_);
}
static void xlsx_polygon(int n, double *x, double *y, const pGEcontext gc,
pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
int i;
Rcpp::NumericVector x_(n);
Rcpp::NumericVector y_(n);
for(i = 0 ; i < n ; i++ ){
x_[i] = x[i];
y_[i] = y[i];
}
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polygon();
//
Rcpp::NumericVector x__ = xlsx_obj->clp->get_x();
Rcpp::NumericVector y__ = xlsx_obj->clp->get_y();
for(i = 0 ; i < y__.size() ; i++ ){
x__[i] += xlsx_obj->offx;
y__[i] += xlsx_obj->offy;
}
xfrm xfrm_(x__, y__);
line_style line_style_(gc->lwd, gc->col, gc->lty, gc->ljoin, gc->lend);
a_color fill_( gc->fill );
fputs("<xdr:sp>", xlsx_obj->file);
write_nv_pr_xlsx(dd, "pg");
fputs("<xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s", xfrm_.xml().c_str());
fputs("<a:custGeom><a:avLst/>", xlsx_obj->file );
fputs( "<a:pathLst>", xlsx_obj->file );
fprintf(xlsx_obj->file, "%s", a_path::a_tag(x__, y__, 1 ).c_str());
fputs( "</a:pathLst>", xlsx_obj->file );
fputs("</a:custGeom>", xlsx_obj->file );
if( fill_.is_visible() > 0 )
fprintf(xlsx_obj->file, "%s", fill_.solid_fill().c_str());
fprintf(xlsx_obj->file, "%s", line_style_.a_tag().c_str());
fputs("</xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s",empty_body_text::x_tag().c_str());
fputs("</xdr:sp>", xlsx_obj->file);
}
static void xlsx_line(double x1, double y1, double x2, double y2,
const pGEcontext gc, pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(2);
Rcpp::NumericVector y_(2);
x_[0] = x1;
y_[0] = y1;
x_[1] = x2;
y_[1] = y2;
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polyline();
std::vector<NumericVector> x_array = xlsx_obj->clp->get_x_lines();
std::vector<NumericVector> y_array = xlsx_obj->clp->get_y_lines();
for( size_t l = 0 ; l < x_array.size() ; l++ ){
xlsx_do_polyline(x_array.at(l), y_array.at(l), gc, dd);
}
}
static void xlsx_polyline(int n, double *x, double *y, const pGEcontext gc,
pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(n);
Rcpp::NumericVector y_(n);
for(int i = 0 ; i < n ; i++ ){
x_[i] = x[i];
y_[i] = y[i];
}
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polyline();
std::vector<NumericVector> x_array = xlsx_obj->clp->get_x_lines();
std::vector<NumericVector> y_array = xlsx_obj->clp->get_y_lines();
for( size_t l = 0 ; l < x_array.size() ; l++ ){
xlsx_do_polyline(x_array.at(l), y_array.at(l), gc, dd);
}
}
// Perform one iteration of the Kalman Filter for a CAR(1) process to update it
void KalmanFilter1::Update() {
double rho, var_ratio, previous_var;
rho = exp(-1.0 * omega_ * dt_(current_index_-1));
previous_var = var(current_index_-1) - yerr_(current_index_-1) * yerr_(current_index_-1);
var_ratio = previous_var / var(current_index_-1);
// Update the Kalman filter mean
mean(current_index_) = rho * mean(current_index_-1) +
rho * var_ratio * (y_(current_index_-1) - mean(current_index_-1));
// Update the Kalman filter variance
var(current_index_) = sigsqr_ / (2.0 * omega_) * (1.0 - rho * rho) +
rho * rho * previous_var * (1.0 - var_ratio);
// add in contribution to variance from measurement errors
var(current_index_) += yerr_(current_index_) * yerr_(current_index_);
current_index_++;
}
void FastOnlineSupervisedMStep<Scalar>::doc_m_step(
const std::shared_ptr<corpus::Document> doc,
const std::shared_ptr<parameters::Parameters> v_parameters,
std::shared_ptr<parameters::Parameters> m_parameters
) {
// Data from document doc
const Eigen::VectorXi & X = doc->get_words();
int y = std::static_pointer_cast<corpus::ClassificationDocument>(doc)->get_class();
// Variational parameters
const MatrixX & phi = std::static_pointer_cast<parameters::VariationalParameters<Scalar> >(v_parameters)->phi;
const VectorX &gamma = std::static_pointer_cast<parameters::VariationalParameters<Scalar> >(v_parameters)->gamma;
// Supervised model parameters
const VectorX &alpha = std::static_pointer_cast<parameters::SupervisedModelParameters<Scalar> >(m_parameters)->alpha;
// Initialize our variables
if (b_.rows() == 0) {
b_ = MatrixX::Zero(phi.rows(), phi.cols());
expected_z_bar_ = MatrixX::Zero(phi.rows(), minibatch_size_);
y_ = Eigen::VectorXi::Zero(minibatch_size_);
eta_velocity_ = MatrixX::Zero(phi.rows(), num_classes_);
eta_gradient_ = MatrixX::Zero(phi.rows(), num_classes_);
}
// Unsupervised sufficient statistics
b_.array() += phi.array().rowwise() * X.cast<Scalar>().transpose().array();
// Supervised suff stats
expected_z_bar_.col(docs_seen_so_far_) = gamma - alpha;
expected_z_bar_.col(docs_seen_so_far_).array() /= expected_z_bar_.col(docs_seen_so_far_).sum();
y_(docs_seen_so_far_) = y;
// mark another document as seen
docs_seen_so_far_++;
// Check if we need to update the parameters
if (docs_seen_so_far_ >= minibatch_size_)
m_step(m_parameters);
}
// Update the coefficients need for computing the Kalman Filter at future times as a function of the
// time series value at some earlier time
void KalmanFilterp::UpdateCoefs() {
kalman_gain_ = PredictionVar_ * rotated_ma_coefs_.t() / var(current_index_-1);
// update the coefficients for predicting the state vector at coefs(i|i-1) --> coefs(i|i)
state_const_ += kalman_gain_ * (y_(current_index_-1) - yconst_);
state_slope_ -= kalman_gain_ * yslope_;
// update the state one-step prediction error variance
PredictionVar_ -= var(current_index_-1) * (kalman_gain_ * kalman_gain_.t());
// compute the one-step state prediction coefficients: coefs(i|i) --> coefs(i+1|i)
rho_ = arma::exp(omega_ * dt_(current_index_-1));
state_const_ = rho_ % state_const_;
state_slope_ = rho_ % state_slope_;
// update the predicted state covariance matrix
PredictionVar_ = (rho_ * rho_.t()) % (PredictionVar_ - StateVar_) + StateVar_;
// compute the coefficients for the linear filter at time[ipredict], and compute the variance in the predicted
// y[ipredict]
yconst_ = std::real( arma::as_scalar(rotated_ma_coefs_ * state_const_) );
yslope_ = std::real( arma::as_scalar(rotated_ma_coefs_ * state_slope_) );
var(current_index_) = std::real( arma::as_scalar(rotated_ma_coefs_ * PredictionVar_ * rotated_ma_coefs_.t()) )
+ yerr_(current_index_) * yerr_(current_index_);
current_index_++;
}
static void xlsx_rect(double x0, double y0, double x1, double y1,
const pGEcontext gc, pDevDesc dd) {
XLSX_dev *xlsx_obj = (XLSX_dev*) dd->deviceSpecific;
Rcpp::NumericVector x_(4);
Rcpp::NumericVector y_(4);
x_[0] = x0;
y_[0] = y0;
x_[1] = x1;
y_[1] = y0;
x_[2] = x1;
y_[2] = y1;
x_[3] = x0;
y_[3] = y1;
xlsx_obj->clp->set_data(x_, y_);
xlsx_obj->clp->clip_polygon();
Rcpp::NumericVector x__ = xlsx_obj->clp->get_x();
Rcpp::NumericVector y__ = xlsx_obj->clp->get_y();
for(int i = 0 ; i < x__.size() ; i++ ){
x__[i] += xlsx_obj->offx;
y__[i] += xlsx_obj->offy;
}
xfrm xfrm_(x__, y__);
line_style line_style_(gc->lwd, gc->col, gc->lty, gc->ljoin, gc->lend);
a_color fill_( gc->fill );
fputs("<xdr:sp>", xlsx_obj->file);
write_nv_pr_xlsx(dd, "rc");
fputs("<xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s", xfrm_.xml().c_str());
fprintf(xlsx_obj->file,"%s", a_prstgeom::a_tag("rect").c_str());
if( fill_.is_visible() > 0 )
fprintf(xlsx_obj->file, "%s", fill_.solid_fill().c_str());
fprintf(xlsx_obj->file, "%s", line_style_.a_tag().c_str());
fputs("</xdr:spPr>", xlsx_obj->file);
fprintf(xlsx_obj->file, "%s",empty_body_text::x_tag().c_str());
fputs("</xdr:sp>", xlsx_obj->file);
}
ColumnVector RateModel::ExpectedValueGet() const {
#ifdef USE_RATE_SYSTEM_MODEL
const double qw = x_(QUATERNION_W);
const double qx = x_(QUATERNION_X);
const double qy = x_(QUATERNION_Y);
const double qz = x_(QUATERNION_Z);
y_(1) = (qw*qw+qx*qx-qy*qy-qz*qz) * x_(RATE_X) + (2.0*qx*qy+2.0*qw*qz) * x_(RATE_Y) + (2.0*qx*qz-2.0*qw*qy) * x_(RATE_Z) + x_(BIAS_GYRO_X);
y_(2) = (2.0*qx*qy-2.0*qw*qz) * x_(RATE_X) + (qw*qw-qx*qx+qy*qy-qz*qz) * x_(RATE_Y) + (2.0*qy*qz+2.0*qw*qx) * x_(RATE_Z) + x_(BIAS_GYRO_Y);
y_(3) = (2.0*qx*qz+2.0*qw*qy) * x_(RATE_X) + (2.0*qy*qz-2.0*qw*qx) * x_(RATE_Y) + (qw*qw-qx*qx-qy*qy+qz*qz) * x_(RATE_Z) + x_(BIAS_GYRO_Z);
#else // USE_RATE_SYSTEM_MODEL
y_(1) = 0.0;
y_(2) = 0.0;
y_(3) = 0.0;
#endif // USE_RATE_SYSTEM_MODEL
return y_;
}
void
arrangement::compute_pointInCells(Arrangement_2 &arr, std::vector<std::vector<double> > &points)
{
Walk_pl walk_pl(arr);
int cpt = 0;
for (Arrangement_2::Face_iterator face = arr.faces_begin(); face != arr.faces_end(); ++face)
{
if (face->is_unbounded())
face->set_data(-1);
else
{
// set data to each face
face->set_data(cpt++);
// find a point in this face
Arrangement_2::Ccb_halfedge_circulator previous = face->outer_ccb();
Arrangement_2::Ccb_halfedge_circulator first_edge = face->outer_ccb();
Arrangement_2::Ccb_halfedge_circulator edge = face->outer_ccb();
++edge;
do
{
std::vector<double> p1 = getPointMiddle(previous);
std::vector<double> p2 = getPointMiddle(edge);
std::vector<double> m;
m.push_back((p1[0]+p2[0])/2);
m.push_back((p1[1]+p2[1])/2);
Rational x_(m[0]);
Rational y_(m[1]);
Conic_point_2 p(x_,y_);
Arrangement_2::Vertex_handle v = insert_point(arr, p, walk_pl);
try
{
if (v->face()->data() == (cpt-1))
{
bool flag = false;
// test if it is not in holes and not in unbounded face
for (int i = 0; i < (int) convolutions_o.size(); ++i)
{
Walk_pl wpl(convolutions_o[i]);
Arrangement_2::Vertex_handle t = insert_point(convolutions_o[i], p, wpl);
if (t->face()->data() == 1)
{
convolutions_o[i].remove_isolated_vertex(t);
break;
}
else if (t->face()->data() == 2 || t->face()->data() == 0)
{
flag = true;
convolutions_o[i].remove_isolated_vertex(t);
break;
}
}
// then continue
if (!flag)
points.push_back(m);
else
{
--cpt;
face->set_data(-1);
}
arr.remove_isolated_vertex(v);
break;
}
arr.remove_isolated_vertex(v);
}
catch (const std::exception exn) {}
previous = edge;
++edge;
} while (edge != first_edge);
}
}
}
void CProxyCamera::GetGLMatrices(double glprojmatrix[],
double glmodelviewmatrix[],
const int glvpmatrix[], double znear, double zfar) const
{
double mat[3][4];
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
mat[i][j] = m_KR(i,j);
}
}
mat[0][3] = m_KT[0];
mat[1][3] = m_KT[1];
mat[2][3] = m_KT[2];
Matrix3d LHC = Matrix3d::Zero();
LHC(0, 0) = LHC(1, 1) = LHC(2, 2) = -1;
LHC(0, 0) = 1;
double icpara[3][4], trans[3][4];
if (arParamDecompMat(mat, icpara, trans) < 0)
{
printf("Fatal error: proj decompose failed!\n");
exit(0);
}
Matrix3d R;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
R(i, j) = trans[i][j];
}
}
Matrix3d LHCR = LHC * R;
Matrix4d modelViewMatrix = Matrix4d::Identity();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
modelViewMatrix(i, j) = LHCR(i,j);
}
}
modelViewMatrix(0, 3) = trans[0][3];
modelViewMatrix(1, 3) = trans[1][3];
modelViewMatrix(2, 3) = trans[2][3];
modelViewMatrix(1, 3) = modelViewMatrix(1, 3) * (-1);
modelViewMatrix(2, 3) = modelViewMatrix(2, 3) * (-1);
modelViewMatrix(3, 3) = 1.0;
Matrix4d finalModelM=modelViewMatrix;
for (int i=0;i<16;i++)
{
glmodelviewmatrix[i]=finalModelM(i);
}
double w = glvpmatrix[2];
double h = glvpmatrix[3];
Matrix4d H_inv = Matrix4d::Identity();
H_inv(0, 0) = 2.0 / w;
H_inv(0, 2) = -1;
H_inv(1, 1) = -2.0 / h;
H_inv(1, 2) = 1.0;
H_inv(3, 2) = 1.0;
Matrix3d K = Matrix3d::Zero();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
K(i, j) = icpara[i][j] / icpara[2][2];
}
}
Matrix3d y = K * LHC;
Matrix4d y_ = Matrix4d::Zero();
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
y_(i, j) = y(i,j);
}
}
Matrix4d result = H_inv * (y_);
double C_ = -(zfar + znear) / (zfar - znear);
double D_ = -(2 * zfar * znear) / (zfar - znear);
result(2, 2) = C_;
result(2, 3) = D_;
Matrix4d finalRes=result;
for (int i=0;i<16;i++)
{
glprojmatrix[i]=finalRes(i);
}
}
// Return the predicted value and its variance at time assuming a CAR(1) process
std::pair<double, double> KalmanFilter1::Predict(double time) {
double rho, var_ratio, previous_var;
double ypredict_mean, ypredict_var, yprecision;
unsigned int ipredict = 0;
while (time > time_(ipredict)) {
// find the index where time_ > time for the first time
ipredict++;
if (ipredict == time_.n_elem) {
// time is greater than last element of time_, so do forecasting
break;
}
}
// Run the Kalman filter up to the point time_[ipredict-1]
Reset();
for (int i=1; i<ipredict; i++) {
Update();
}
if (ipredict == 0) {
// backcasting, so initialize the conditional mean and variance to the stationary values
ypredict_mean = 0.0;
ypredict_var = sigsqr_ / (2.0 * omega_);
} else {
// predict the value of the time series at time, given the earlier values
double dt = time - time_[ipredict-1];
rho = exp(-dt * omega_);
previous_var = var(ipredict-1) - yerr_(ipredict-1) * yerr_(ipredict-1);
var_ratio = previous_var / var(ipredict-1);
// initialize the conditional mean and variance
ypredict_mean = rho * mean(ipredict-1) + rho * var_ratio * (y_(ipredict-1) - mean(ipredict-1));
ypredict_var = sigsqr_ / (2.0 * omega_) * (1.0 - rho * rho) + rho * rho * previous_var * (1.0 - var_ratio);
}
if (ipredict == time_.n_elem) {
// Forecasting, so we're done: no need to run interpolation steps
std::pair<double, double> ypredict(ypredict_mean, ypredict_var);
return ypredict;
}
yprecision = 1.0 / ypredict_var;
ypredict_mean *= yprecision;
// Either backcasting or interpolating, so need to calculate coefficients of linear filter as a function of
// the predicted time series value, then update the running conditional mean and variance of the predicted
// time series value
InitializeCoefs(time, ipredict, 0.0, 0.0);
yprecision += yslope_ * yslope_ / var(ipredict);
ypredict_mean += yslope_ * (y_(ipredict) - yconst_) / var(ipredict);
for (int i=ipredict+1; i<time_.n_elem; i++) {
UpdateCoefs();
yprecision += yslope_ * yslope_ / var(i);
ypredict_mean += yslope_ * (y_(i) - yconst_) / var(i);
}
ypredict_var = 1.0 / yprecision;
ypredict_mean *= ypredict_var;
std::pair<double, double> ypredict(ypredict_mean, ypredict_var);
return ypredict;
}
// Predict the time series at the input time given the measured time series, assuming a CARMA(p,q) process
std::pair<double, double> KalmanFilterp::Predict(double time) {
unsigned int ipredict = 0;
while (time > time_(ipredict)) {
// find the index where time_ > time for the first time
ipredict++;
if (ipredict == time_.n_elem) {
// time is greater than last element of time_, so do forecasting
break;
}
}
// Run the Kalman filter up to the point time_[ipredict-1]
Reset();
for (int i=1; i<ipredict; i++) {
Update();
}
double ypredict_mean, ypredict_var, yprecision;
if (ipredict == 0) {
// backcasting, so initialize the conditional mean and variance to the stationary values
ypredict_mean = 0.0;
ypredict_var = std::real( arma::as_scalar(rotated_ma_coefs_ * StateVar_ * rotated_ma_coefs_.t()) );
} else {
// predict the value of the time series at time, given the earlier values
kalman_gain_ = PredictionVar_ * rotated_ma_coefs_.t() / var(ipredict-1);
state_vector_ += kalman_gain_ * innovation_;
PredictionVar_ -= var(ipredict-1) * (kalman_gain_ * kalman_gain_.t());
double dt = std::abs(time - time_(ipredict-1));
rho_ = arma::exp(omega_ * dt);
state_vector_ = rho_ % state_vector_;
PredictionVar_ = (rho_ * rho_.t()) % (PredictionVar_ - StateVar_) + StateVar_;
// initialize the conditional mean and variance
ypredict_mean = std::real( arma::as_scalar(rotated_ma_coefs_ * state_vector_) );
ypredict_var = std::real( arma::as_scalar(rotated_ma_coefs_ * PredictionVar_ * rotated_ma_coefs_.t()) );
}
if (ipredict == time_.n_elem) {
// Forecasting, so we're done: no need to run interpolation steps
std::pair<double, double> ypredict(ypredict_mean, ypredict_var);
return ypredict;
}
yprecision = 1.0 / ypredict_var;
ypredict_mean *= yprecision;
// Either backcasting or interpolating, so need to calculate coefficients of linear filter as a function of
// the predicted time series value, then update the running conditional mean and variance of the predicted
// time series value
InitializeCoefs(time, ipredict, ypredict_mean / yprecision, ypredict_var);
yprecision += yslope_ * yslope_ / var(ipredict);
ypredict_mean += yslope_ * (y_(ipredict) - yconst_) / var(ipredict);
for (int i=ipredict+1; i<time_.n_elem; i++) {
UpdateCoefs();
yprecision += yslope_ * yslope_ / var(i);
ypredict_mean += yslope_ * (y_(i) - yconst_) / var(i);
}
ypredict_var = 1.0 / yprecision;
ypredict_mean *= ypredict_var;
std::pair<double, double> ypredict(ypredict_mean, ypredict_var);
return ypredict;
}
void
arrangement::compute_ACScell()
{
/*
for (int i = 0; i < (int)convolutions.size(); ++i)
for (Arrangement_2::Edge_iterator edge = convolutions[i].edges_begin(); edge != convolutions[i].edges_end(); ++edge)
{
convolution_r_all = Arrangement_2(convolutions[i]);
insert(convolution_r_all, edge->curve());
Arrangement_2::Edge_iterator e = convolution_r_all.edges_end();
e->set_data(edge->data());
}
*/
/*
std::cout << "edge label convo : ";
for (Arrangement_2::Edge_iterator edge = convolution_r_all.edges_begin(); edge != convolution_r_all.edges_end(); ++edge)
std::cout << edge->data() << " ";
std::cout << std::endl;
*/
for (int i = 0; i < (int)point_in_faces.size(); ++i)
{
// do a copy
Arrangement_2 copy(convolutions[0]);
Observer observer(copy);
// add a circle
Rat_point_2 center(point_in_faces[i][0], point_in_faces[i][1]);
Rat_circle_2 circle(center, r1r2 * r1r2);
Conic_curve_2 conic_arc(circle);
insert(copy, conic_arc);
// then add rho label for arc
for (Arrangement_2::Edge_iterator edge = copy.edges_begin(); edge != copy.edges_end(); ++edge)
{
if (edge->data().compare("") == 0)
{
edge->set_data("rho_");
edge->twin()->set_data("rho_");
}
}
Walk_pl walk_pl(copy);
std::vector<std::vector<double> > points;
compute_pointInCells(copy, points);
for (int j = 0; j < (int)points.size(); ++j)
{
double _x = point_in_faces[i][0] - points[j][0];
double _y = point_in_faces[i][1] - points[j][1];
if (r1r2 < sqrt(_x * _x + _y * _y))
{
Rational x_(points[j][0]);
Rational y_(points[j][1]);
Conic_point_2 p(x_,y_);
Arrangement_2::Vertex_handle v = insert_point(copy, p, walk_pl);
try
{
ACScell cell(i);
Arrangement_2::Ccb_halfedge_circulator first_outer_ccb = v->face()->outer_ccb();
Arrangement_2::Ccb_halfedge_circulator outer_ccb = v->face()->outer_ccb();
do
{
// for retrieving ACScell Begin and End
insert(cell.arr, outer_ccb->curve());
// then continue
cell.addLabel(outer_ccb->data());
++outer_ccb;
} while (outer_ccb != first_outer_ccb);
ACScells.push_back(cell);
}
catch (const std::exception exn) {}
}
}
}
// clean ACScells
for (int i = 0; i < (int)ACScells.size(); ++i)
ACScells[i].cleanLabels();
// compute ACScells id
int cpt = 0;
int previous = 0;
for (int i = 0; i < (int)ACScells.size(); ++i)
{
if (previous != ACScells[i].NCR)
{
cpt = 0;
previous = ACScells[i].NCR;
}
ACScells[i].id = std::to_string(ACScells[i].NCR) + "." + std::to_string(cpt);
++cpt;
}
}
//-----------------------------------------------------------------------------
static void TestEarth(char sTestName[],
double (__cdecl *func)(const double xrow[], const int iResponse),
const int nCases, const int nResponses, const int nPreds,
const int nMaxDegree, const int nMaxTerms,
const double Trace, const bool Format,
const double ForwardStepThresh,
const int nFastK, const double FastBeta, const double NewVarPenalty,
const int seed,
const double Collinear = 0) // used for testing NewVarPenalty
{
#define y_(i,iResponse) y[(i) + (iResponse)*(nCases)]
int *LinPreds = (int *)calloc(nPreds, sizeof(int));
double *x = (double *)malloc(nCases * nPreds * sizeof(double));
double *y = (double *)malloc(nCases * nResponses * sizeof(double));
double *bx = (double *)malloc(nCases * nMaxTerms * sizeof(double));
bool *BestSet = (bool *) malloc(nMaxTerms * sizeof(bool));
int *Dirs = (int *) malloc(nMaxTerms * nPreds * sizeof(int));
double *Cuts = (double *)malloc(nMaxTerms * nPreds * sizeof(double));
double *Residuals = (double *)malloc(nCases * nResponses * sizeof(double));
double *Betas = (double *)malloc(nMaxTerms * nResponses * sizeof(double));
static int nTest;
nTest++;
printf("=============================================================================\n");
printf("TEST %d: %s n=%d p=%d\n", nTest, sTestName, nCases, nPreds);
// init x
srand(seed);
int i;
for (i = 0; i < nCases; i++)
for (int iPred = 0; iPred < nPreds; iPred++) {
double xtemp;
xtemp = (double)((rand() % 20000) - 10000) / 10000; // rand number from -1 to +1
x[i + iPred * nCases] = xtemp;
}
// sort the first column of x, makes debugging easier
qsort(x, nCases, sizeof(double), cmp);
if (Collinear > 0) {
// copy column 0 to 1 with added noise
for (i = 0; i < nCases; i++)
x[i + 1 * nCases] = x[i] + Collinear * RandGauss();
}
// init y
double *xrow = (double *)malloc(nPreds * sizeof(double));
for (i = 0; i < nCases; i++) {
for (int iPred = 0; iPred < nPreds; iPred++)
xrow[iPred] = x[i + iPred * nCases];
for (int iResponse = 0; iResponse < nResponses; iResponse++)
y_(i, iResponse) = func(xrow, iResponse);
}
free(xrow);
double BestGcv;
int nTerms, iReason;
const double Penalty = ((nMaxDegree>1)? 3:2);
clock_t Time = clock();
if(Trace >= 4) {
if(nResponses != 1)
error("cannot use Trace>=4 with nResponses!=1");
printf(" y");
for(int iPred = 0; iPred < nPreds; iPred++)
printf(" x%d", iPred);
printf("\n");
for(int i = 0; i < nCases; i++) {
printf("%4d % 7.5f", i, y[i]);
for(int iPred = 0; iPred < nPreds; iPred++) {
printf(" % 7.5f", x[i + iPred * nCases]);
}
printf("\n");
}
printf("\n");
}
Earth(&BestGcv, &nTerms, &iReason, BestSet, bx, Dirs, Cuts, Residuals, Betas,
x, y, NULL, // weights are NULL
nCases, nResponses, nPreds, nMaxDegree, nMaxTerms, Penalty, ForwardStepThresh,
0, 0, // MinSpan, EndSpan
true, // Prune
nFastK, FastBeta, NewVarPenalty, LinPreds,
2 /*AdjustEndSpan*/, true /*AutoLinPred*/, true /*UseBetaCache*/,
Trace, NULL);
// calc nUsedTerms
int nUsedTerms = 0;
for (int iTerm = 0; iTerm < nTerms; iTerm++)
if (BestSet[iTerm])
nUsedTerms++;
// calc RSquared, GRSquared
for (int iResponse = 0; iResponse < nResponses; iResponse++) {
double Rss = 0, Tss = 0, meanY = 0;
for (i = 0; i < nCases; i++)
meanY += y_(i, iResponse);
meanY /= nCases;
xrow = (double *)malloc(nPreds * sizeof(double));
double *yHat = (double *)malloc(nResponses * sizeof(double));
for (i = 0; i < nCases; i++) {
for (int iPred = 0; iPred < nPreds; iPred++)
xrow[iPred] = x[i + iPred * nCases];
PredictEarth(yHat, xrow, BestSet, Dirs, Cuts, Betas, nPreds, nResponses, nTerms, nMaxTerms);
double Residual = y_(i,iResponse) - yHat[iResponse];
Rss += sq(Residual);
Tss += sq(y_(i,iResponse) - meanY);
}
free(yHat);
free(xrow);
const double RSq = 1 - Rss/Tss;
const double GcvNull = getGcv(1, nCases, Tss, Penalty);
const double GRSq = 1 - getGcv(nUsedTerms, nCases, Rss, Penalty) / GcvNull;
#if PRINT_TIME
double TimeDelta = (double)(clock() - Time) / CLOCKS_PER_SEC;
#else
double TimeDelta = 99.99;
#endif
// show results
if (nResponses > 1) {
printf("RESULT %d Response %d: GRSq %.5g RSq %.5g nTerms %d of %d of %d",
nTest, iResponse+1, GRSq, RSq,
nUsedTerms, nTerms, nMaxTerms);
if (iResponse == 0)
printf(" FUNCTION %s n=%d p=%d [%.2f secs]",
sTestName, nCases, nPreds, TimeDelta);
printf("\n");
}
else
printf("RESULT %d: GRSq %g RSq %g nTerms %d of %d of %d "
"FUNCTION %s n=%d p=%d [%.2f secs]\n",
nTest, GRSq, RSq, nUsedTerms, nTerms, nMaxTerms,
sTestName, nCases, nPreds, TimeDelta);
}
if (Format && Trace != 0) {
printf("\nTEST %d: FUNCTION %s n=%d p=%d\n", nTest, sTestName, nCases, nPreds);
FormatEarth(BestSet, Dirs, Cuts, Betas, nPreds, nResponses, nTerms, nMaxTerms, 3, 1e-6);
printf("\n");
}
free(LinPreds);
free(x);
free(y);
free(BestSet);
free(Dirs);
free(Cuts);
free(Residuals);
free(Betas);
free(bx);
}
/*--------------------------------------------------------------*/
void nrn_scopmath_solve_thread(int n, double** a, double* b,
int* perm, double* p, int* y)
#define y_(arg) p[y[arg]]
#define b_(arg) b[arg]
{
int i, j, pivot;
double sum;
/* Perform forward substitution with pivoting */
if (y) {
for (i = 0; i < n; i++)
{
pivot = perm[i];
sum = 0.0;
for (j = 0; j < i; j++)
sum += a[pivot][j] * (y_(j));
y_(i) = (b_(pivot) - sum) / a[pivot][i];
}
/*
* Note that the y vector is already in the correct order for back
* substitution. Perform back substitution, pivoting the matrix but not
* the y vector. There is no need to divide by the diagonal element as
* this is assumed to be unity.
*/
for (i = n - 1; i >= 0; i--)
{
pivot = perm[i];
sum = 0.0;
for (j = i + 1; j < n; j++)
sum += a[pivot][j] * (y_(j));
y_(i) -= sum;
}
}else{
for (i = 0; i < n; i++)
{
pivot = perm[i];
sum = 0.0;
for (j = 0; j < i; j++)
sum += a[pivot][j] * (p[j]);
p[i] = (b_(pivot) - sum) / a[pivot][i];
}
/*
* Note that the y vector is already in the correct order for back
* substitution. Perform back substitution, pivoting the matrix but not
* the y vector. There is no need to divide by the diagonal element as
* this is assumed to be unity.
*/
for (i = n - 1; i >= 0; i--)
{
pivot = perm[i];
sum = 0.0;
for (j = i + 1; j < n; j++)
sum += a[pivot][j] * (p[j]);
p[i] -= sum;
}
}
}
ColumnVector BaroModel::ExpectedValueGet() const
{
y_(1) = qnh_ * pow(1.0 - (0.0065 * (x_(POSITION_Z) + elevation_)) / 288.15, 5.255);
return y_;
}
