void Force_iid_model::update(arma::colvec& Y,const arma::colvec3& force, const arma::colvec3& torque) {
features(contact) = gaussian_step_function_positive(arma::norm(force),0.9,1,beta);
features(up) = gaussian_step_function_positive(force(0),limits[contact].lower,limits[contact].upper,var[contact]); //+
features(down) = gaussian_step_function_negative(force(0),limits[down].lower,limits[down].upper,var[down]); //-
features(left) = gaussian_step_function_negative(force(1),limits[left].lower,limits[left].upper,var[left]); // -
features(right) = gaussian_step_function_positive(force(1),limits[right].lower,limits[right].upper,var[right]); // +
features(push) = gaussian_step_function_negative(force(2),limits[push].lower,limits[push].upper,var[push]); //-
//contact,up,down,left,right,push
features_simple(0) = features(contact);
features_simple(1) = arma::max(features(arma::span(1,4)));
if(type == FULL) {
Y.resize(6);
Y(contact) = features(contact);
Y(up) = features(up);
Y(down) = features(down);
Y(left) = features(left);
Y(right) = features(right);
Y(push) = features(push);
} else {
Y.resize(2);
Y(0) = features_simple(0);
Y(1) = features_simple(1);
}
}
// [[Rcpp::export]]
double ksFastTestStatistic(arma::colvec y, arma::colvec z){
uvec trt = find( z == 1 );
uvec ctrl = find( z == 0 );
vec yT = y.elem(trt);
vec yC = y.elem(ctrl);
// assuming no missing vals yT = yT[!is.na(yT)];
double nX = yT.n_rows; //length(yT);
double nY = yC.n_rows;
double n = nX * nY/(nX + nY);
colvec w = join_cols(yT, yC); // c(yT,yC)
uvec orderedw = sort_index(w); //order(w)
int totalN = nX + nY;
colvec d(totalN);
d.fill(1/nX);
d.elem( find( orderedw > nX ) ).fill(-1/nY);
// d = cumsum(Rcpp::ifelse(orderedw <= nX, 1/nX, -1/nY));
colvec F = cumsum(d);
// check for ties
vec uniqF = unique(w);
vec out = F;
if ( uniqF.n_elem < totalN ){
//uvec nonzerodiffs = find( diff(sort(F)) != 0 );
vec Fnonzerodiffs = F.elem( find( diff(sort(F)) != 0 ) );
vec out = Fnonzerodiffs; // I dont know why I cant use join_cols (Fnonzerodiffs,F(totalN-1))
out.insert_rows(1,F(totalN-1)); // z <- z[c(which(diff(sort(w)) != 0), n.x + n.y)]
}
// return(max(abs(out)));
return(max(abs(out)));
}
double get_xi_sqr(arma::colvec &x, arma::mat &post_sigma, arma::colvec &post_mu){
arma::mat v = x.t()*post_sigma*x;
arma::mat xmu = x.t()*post_mu;
std::cout << "x.t()*post_sigma*x = " << std::endl;
v.print();
std::cout << "x.t()*post_mu = " << std::endl;
xmu.print();
return as_scalar(v) + as_scalar(xmu)*as_scalar(xmu);
}
bool CamAruco2DObservationModel::hasClearLineOfSight(const ompl::base::State *state, const arma::colvec& landmark )
{
using namespace arma;
colvec xVec = state->as<SE2BeliefSpace::StateType>()->getArmaData();
colvec robot_to_landmark_ray = landmark.subvec(1,2) - xVec.subvec(0,1);
double distance = norm(robot_to_landmark_ray,2);
int steps = std::floor(distance/ompl::magic::ONE_STEP_DISTANCE_FOR_VISIBILITY);
ompl::base::State *tempState = this->si_->allocState();
for(int i=1 ; i < steps; i++)
{
double newX = xVec(0) + i*robot_to_landmark_ray(0)/steps;
double newY = xVec(1) + i*robot_to_landmark_ray(1)/steps;
tempState->as<SE2BeliefSpace::StateType>()->setXYYaw(newX, newY,0);
if(!this->si_->isValid(tempState))
{
return false;
}
}
si_->freeState(tempState);
return true;
}
void CamAruco2DObservationModel::calculateRangeBearingToLandmark(const ompl::base::State *state, const arma::colvec& landmark, double& range, double& bearing)
{
using namespace arma;
colvec xVec = state->as<SE2BeliefSpace::StateType>()->getArmaData();
colvec diff = landmark.subvec(1,2) - xVec.subvec(0,1);
//norm is the 2nd Holder norm, i.e. the Euclidean norm
double distance = norm(diff,2);
double robot_landmark_ray = atan2(diff[1],diff[0]) ;
double delta_theta = robot_landmark_ray - xVec[2];
//if(distance < 0.1)
//{
// OMPL_INFORM("Aruco: Range: %f bearing: %f", distance, delta_theta);
//}
FIRMUtils::normalizeAngleToPiRange(delta_theta);
range = distance;
bearing = delta_theta;
}
// //' Self-concordant Taylor approximation logstar function
// //'
// //' Minus log and its first \code{der} derivatives, on \code{eps < x < M}, with
// //' fourth order Taylor series approximation to the left of \code{eps} and to the right of \code{M}
// //'
// //' @param x column vector of observations
// //' @param eps lower tolerance
// //' @param M maximum value
// //' @param der derivative, either 0, 1 or 2.
// //'
// //' @return a matrix with \eqn{der+1} columns containing values
arma::mat mllog(arma::colvec x, double eps, double M, int der=0){
if(!(der==0 || der==1 || der==2)){
Rcpp::stop("Only first two derivatives and log functions implemented");
}
if(eps > M){
Rcpp::stop("Thresholds out of order");
}
//Coefficients for 4th order Taylor approx below eps
arma::vec coefs(5);
arma::vec Coefs(5);
coefs(0) = -log(eps);
Coefs(0) = -log(M);
for(int i=1; i<5; i++){
coefs(i) = R_pow(-eps,-i)/(double)i;
Coefs(i) = R_pow(-M,-i)/(double)i;
}
arma::uvec lo = find(x < eps);
arma::uvec hi = find(x > M);
//Function value
arma::vec f(x.size());
f = -log(x);
f(lo) = h(x(lo)-eps, coefs);
f(hi) = h(x(hi)-M, Coefs);
if(der < 1){
return f;
}
//First derivative
arma::vec fp(x.size());
fp = -1.0/x;
fp(lo) = hp(x(lo)-eps, eps);
fp(hi) = hp(x(hi)-M, M);
if(der < 2){
return join_rows(f, fp);
}
//Second derivative
arma::vec fpp(x.size());
fpp = pow(x,-2);
fpp(lo) = hpp(x(lo)-eps, eps);
fpp(hi) = hpp(x(hi)-M, M);
return join_rows(join_rows(f,fp),fpp);
}
arma::colvec computeBetaJ_cpp(arma::colvec y, arma::mat X, int j, int m, int adjFinSample) {
int bigT = X.n_rows;
int numThrow = ceil((double)m / 2);
if (adjFinSample == 0) numThrow = m ;
mat XSparse; colvec ySparse;
XSparse.zeros(bigT - numThrow, X.n_cols); ySparse.zeros(bigT - numThrow);
if (j < 2) {
XSparse.rows(0, bigT - numThrow - 1) = X.rows(numThrow, bigT - 1);
ySparse.rows(0, bigT - numThrow - 1) = y.rows(numThrow, bigT - 1);
} else {
XSparse.rows(0, j - 2) = X.rows(0, j - 2); ySparse.rows(0, j - 2) = y.rows(0, j - 2);
XSparse.rows(j - 1, bigT - numThrow - 1) = X.rows(j + numThrow - 1, bigT - 1);
ySparse.rows(j - 1, bigT - numThrow - 1) = y.rows(j + numThrow - 1, bigT - 1);
}
colvec beta = runOLSFast_cpp(ySparse, XSparse);
return(beta);
}
void GMM::expection(arma::colvec& x) const{
x.zeros();
for(std::size_t k = 0; k < K;k++){
x += Means[k] * pi(k);
}
}
void CheckActivationCorrect(const arma::colvec input, const arma::colvec target)
{
// Test the activation function using a single value as input.
for (size_t i = 0; i < target.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(ActivationFunction::fn(input.at(i)),
target.at(i), 1e-3);
}
// Test the activation function using the entire vector as input.
arma::colvec activations;
ActivationFunction::fn(input, activations);
for (size_t i = 0; i < activations.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
}
}
void CheckDerivativeCorrect(const arma::colvec input, const arma::colvec target)
{
// Test the calculation of the derivatives using a single value as input.
for (size_t i = 0; i < target.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(ActivationFunction::deriv(input.at(i)),
target.at(i), 1e-3);
}
// Test the calculation of the derivatives using the entire vector as input.
arma::colvec derivatives;
ActivationFunction::deriv(input, derivatives);
for (size_t i = 0; i < derivatives.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
}
}
//[[Rcpp::export]]
double computeSStat_cpp(arma::colvec beta, arma::mat invS, arma::colvec y, arma::mat X,
int j, int m) {
mat newX = X.rows(j - 1, j + m - 2); colvec newY = y.rows(j - 1, j + m - 2);
mat A = trans(newX) * invS * (newY - newX * beta);
mat V = trans(newX) * invS * newX;
mat S = trans(A) * inv(V) * A;
double s = S(0, 0);
return(s);
}
double nLL2(arma::colvec par, arma::colvec Data, int Nf) {
double failcomp=0.0,suscomp=0.0;
arma::colvec F=Data.rows(0,Nf-1);
for(int i=0; i<Nf; i++) {
failcomp=failcomp+R::dlnorm(F(i),par(0),par(1),1);
}
failcomp=-failcomp;
int Nd=Data.n_rows;
arma::colvec S;
if(Nd>Nf) {
S=Data.rows(Nf,Nd-1);
for(int i=0; i<(Nd-Nf); i++) {
suscomp=suscomp+R::plnorm(S(i),par(0),par(1),0,1);
}
}
double negLL=failcomp-suscomp;
return negLL;
}
//--------------------------------------------------------------------------------------------------
double Krigidx( const arma::colvec& KF,
const arma::colvec& comb,
const arma::mat& X,
const arma::cube& Gamma ) {
int k;
int n = X.n_rows;
int c = comb.size();
double S;
arma::mat W = arma::ones( n, n );
for ( k = 0; k < c; k++ ) {
W = W % Gamma.slice( comb( k ) - 1 );
}
S = as_scalar( KF.t() * W * KF );
return S;
}
int classify(double xi, arma::mat &sigma, arma::mat &sigma_inv, arma::colvec &x, arma::colvec &mu){
arma::mat sigma_t_inv = get_post_sigma_inv(xi, x, sigma_inv);
arma::mat sigma_t = sigma_t_inv.i();
arma::colvec mu_t;
int s = 0;
mu_t = get_post_mu(x, sigma_inv, sigma_t, mu, s);
double log_0 = log(sigmoid(xi)) - xi/2 - lambda(xi)*xi*xi - 0.5*as_scalar(mu.t()*sigma_inv*mu) + 0.5*as_scalar(mu_t.t()*sigma_t.t()*mu_t) + 0.5*log(det(sigma_t)/det(sigma));
s = 1;
mu_t = get_post_mu(x, sigma_inv, sigma_t, mu, s);
double log_1 = log(sigmoid(xi)) - xi/2 - lambda(xi)*xi*xi - 0.5*as_scalar(mu.t()*sigma_inv*mu) + 0.5*as_scalar(mu_t.t()*sigma_t.t()*mu_t) + 0.5*log(det(sigma_t)/det(sigma));
if(log_1 > log_0) return 1;
else return 0;
}
void CheckInverseCorrect(const arma::colvec input)
{
// Test the calculation of the inverse using a single value as input.
for (size_t i = 0; i < input.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(ActivationFunction::inv(ActivationFunction::fn(
input.at(i))), input.at(i), 1e-3);
}
// Test the calculation of the inverse using the entire vector as input.
arma::colvec activations;
ActivationFunction::fn(input, activations);
ActivationFunction::inv(activations, activations);
for (size_t i = 0; i < input.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(activations.at(i), input.at(i), 1e-3);
}
}
arma::mat computeCovMatForAndrews_cpp(arma::colvec u, int breakDate) {
int bigT = u.n_rows;
int m = bigT - breakDate;
mat totSum =zeros(m, m);
for (int j = 0; j < (breakDate + 1); j++) {
colvec uUsed = u.rows(j, j + m - 1);
totSum = totSum + uUsed * trans(uUsed);
}
mat Sigma = totSum / (breakDate + 1);
return(Sigma);
}
/*
* Implementation of the PReLU activation function test. The function
* is implemented as PReLU layer in the file perametric_relu.hpp
*
* @param input Input data used for evaluating the PReLU activation
* function.
* @param target Target data used to evaluate the PReLU activation.
*/
void CheckPReLUActivationCorrect(const arma::colvec input,
const arma::colvec target)
{
PReLU<> prelu;
// Test the activation function using the entire vector as input.
arma::colvec activations;
prelu.Forward(std::move(input), std::move(activations));
for (size_t i = 0; i < activations.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(activations.at(i), target.at(i), 1e-3);
}
}
arma::colvec computeTestStatDistribution_cpp(arma::colvec y, arma::mat X, int breakDate,
int m, int adjFinSample, arma::mat invS) {
int bigT = X.n_rows;
colvec sJ = zeros(breakDate - m + 1);
for (int j = 0; j < (breakDate - m + 1); j++) {
colvec betaJ = computeBetaJ_cpp(y.rows(0, bigT - 1 - m), X.rows(0, bigT - 1 - m),
j + 1, m, adjFinSample);
sJ(j) = computeSStat_cpp(betaJ, invS, y, X, j + 1, m);
// if (j == 44) {
// Rcout << "betaJ = "<< betaJ << "sJ(j) = " << sJ(j) << endl;
// }
}
return(sJ);
}
int CamAruco2DObservationModel::findCorrespondingLandmark(const ompl::base::State *state, const arma::colvec &observedLandmark, arma::colvec &candidateObservation)
{
using namespace arma;
int landmarkID = observedLandmark[0];
double maxLikelihood = -1.0;
double candidatelandmarkRange =0, candidatelandmarkBearing = 0;
int candidateIndx = -1;
for(unsigned int i = 0; i < landmarks_.size() ; i++)
{
if(landmarks_[i](0) == landmarkID)
{
double landmarkRange =0, landmarkBearing = 0;
// get range and bearing to landmark
this->calculateRangeBearingToLandmark(state, landmarks_[i], landmarkRange,landmarkBearing);
arma::colvec prediction;
prediction<<landmarkRange<<landmarkBearing<<endr;
// calculate the likelihood
double lkhd = getDataAssociationLikelihood(observedLandmark.subvec(1,2), prediction);
if(lkhd > maxLikelihood)
{
candidateIndx = i;
maxLikelihood = lkhd;
candidatelandmarkRange = landmarkRange;
candidatelandmarkBearing = landmarkBearing;
}
}
}
assert(candidateIndx >= 0 && "Candidate index cannot be negative");
// The observation is id, range, bearing, orientation of the landmark
candidateObservation<<landmarkID<<candidatelandmarkRange<<candidatelandmarkBearing<<landmarks_[candidateIndx][3]<<endr;
assert(candidateIndx>=0);
return candidateIndx;
}
double weight_pow_dist( const arma::colvec& x,
const arma::colvec& y,
const arma::colvec& w,
const arma::colvec& p ) {
double d = 0.0;
if ( x.size() > 0 && x.size() == y.size() && y.size() == w.size() && w.size() == p.size() ) {
int i, n;
n = x.size();
#pragma omp parallel for shared( w, x, y, p ) private( i ) reduction(+:d)
for( i = 0; i < n; i++ ) {
d += w(i) * std::pow( std::abs( x(i) - y(i) ), p(i) );
}
}
return d;
}
/*
* Implementation of the PReLU activation function derivative test.
* The function is implemented as PReLU layer in the file
* perametric_relu.hpp
*
* @param input Input data used for evaluating the PReLU activation
* function.
* @param target Target data used to evaluate the PReLU activation.
*/
void CheckPReLUDerivativeCorrect(const arma::colvec input,
const arma::colvec target)
{
PReLU<> prelu;
// Test the calculation of the derivatives using the entire vector as input.
arma::colvec derivatives;
// This error vector will be set to 1 to get the derivatives.
arma::colvec error = arma::ones<arma::colvec>(input.n_elem);
prelu.Backward(std::move(input), std::move(error), std::move(derivatives));
for (size_t i = 0; i < derivatives.n_elem; i++)
{
BOOST_REQUIRE_CLOSE(derivatives.at(i), target.at(i), 1e-3);
}
}
// mvrGaussian_density evaluates the PDF of the multivariate Gaussian distribution
// plase note this function returns the un-logged PDF to avoid infinite values
// [[Rcpp::export]]
double mvrGaussian_pdf(arma::colvec x, arma::colvec mu, arma::mat sigma){
//define constants
int k = x.size();
double twoPi = 2 * M_PI;
double out;
double rootTerm;
rootTerm = 1 / sqrt(pow(twoPi,k) * det(sigma));
arma::mat AexpTerm;
AexpTerm = exp(-0.5 * arma::trans(x - mu) * inv(sigma) * (x - mu));
double expTerm;
expTerm = AexpTerm(0,0);
out = rootTerm * expTerm;
return(out);
}
//--------------------------------------------------------------------------------------------------
double Krigvar( const arma::colvec& KF,
const arma::cube& Gamma ) {
int i;
int n = Gamma.n_rows;
int m = Gamma.n_slices;
double Var;
arma::mat V = arma::ones( n, n );
for( i = 0; i < m; i++ ) {
V = V % ( arma::ones( n, n ) + Gamma.slice( i ) );
}
V = V - arma::ones( n, n );
Var = as_scalar( KF.t() * V * KF );
return Var;
}
//[[Rcpp::export]]
List gibbsNG(arma::colvec x, int N){
int n= x.size();
arma::mat theta(N,2);
double mi=0, phi=1; /* chutes iniciais */
double a=1, b=1; /* valores para os hiperparâmetros da a priori */
double m=0, v=1;
for(int i=0; i<N; i++)
{
double A = a + 0.5*n ;
double B=0;
for(int j=0; j<n; j++){ B += pow( (x(j,0)-mi) ,2) ; }
B = b + 0.5*B;
phi=R::rgamma(A,1/B); /* atualiza phi com base em A e B */
double V = 1/( (n*phi) + (1/v) );
double M = V*( (m/v) + sum(x)*phi );
mi = R::rnorm(M,V);
theta(i,0)=mi;
theta(i,1)=phi;
}
List saida;
saida["cadeias"]=theta;
return saida;
}
//--------------------------------------------------------------------------------------------------
double linear_kernel( const arma::colvec& x, const arma::colvec& y, const double& alpha ) {
return as_scalar( alpha * x.t() * y );
}
void Peg_distance_model::print(const arma::colvec& Y) const{
Y.print("Y");
}
arma::mat get_post_sigma_inv(double xi, arma::colvec &x, arma::mat &sigma_inv){
std::cout << "x*x.t()" << std::endl;
(x*x.t()).print();
return sigma_inv + 2*std::abs(lambda(xi))*(x*x.t());
}
//' Compute ego/alter edge coordinates considering alter's size and aspect ratio
//'
//' Given a graph, vertices' positions and sizes, calculates the absolute positions
//' of the endpoints of the edges considering the plot's aspect ratio.
//'
//' @param graph A square matrix of size \eqn{n}. Adjacency matrix.
//' @param toa Integer vector of size \eqn{n}. Times of adoption.
//' @param x Numeric vector of size \eqn{n}. x-coordinta of vertices.
//' @param y Numeric vector of size \eqn{n}. y-coordinta of vertices.
//' @param vertex_cex Numeric vector of size \eqn{n}. Vertices' sizes in terms
//' of the x-axis (see \code{\link{symbols}}).
//' @param undirected Logical scalar. Whether the graph is undirected or not.
//' @param no_contemporary Logical scalar. Whether to return (compute) edges'
//' coordiantes for vertices with the same time of adoption (see details).
//' @param dev Numeric vector of size 2. Height and width of the device (see details).
//' @param ran Numeric vector of size 2. Range of the x and y axis (see details).
//' @param curved Logical vector.
//' @return A numeric matrix of size \eqn{m\times 5}{m * 5} with the following
//' columns:
//' \item{x0, y0}{Edge origin}
//' \item{x1, y1}{Edge target}
//' \item{alpha}{Relative angle between \code{(x0,y0)} and \code{(x1,y1)} in terms
//' of radians}
//' With \eqn{m} as the number of resulting edges.
//' @details
//'
//' In order to make the plot's visualization more appealing, this function provides
//' a straight forward way of computing the tips of the edges considering the
//' aspect ratio of the axes range. In particular, the following corrections are
//' made at the moment of calculating the egdes coords:
//'
//' \itemize{
//' \item{Instead of using the actual distance between ego and alter, a relative
//' one is calculated as follows
//' \deqn{d'=\left[(x_0-x_1)^2 + (y_0' - y_1')^2\right]^\frac{1}{2}}{d'=sqrt[(x0-x1)^2 + (y0'-y1')^2]}
//' where \eqn{%
//' y_i'=y_i\times\frac{\max x - \min x}{\max y - \min y} }{%
//' yi' = yi * [max(x) - min(x)]/[max(y) - min(y)]}
//' }
//' \item{Then, for the relative elevation angle, \code{alpha}, the relative distance \eqn{d'}
//' is used, \eqn{\alpha'=\arccos\left( (x_0 - x_1)/d' \right)}{\alpha' = acos[ (x0 - x1)/d' ]}}
//' \item{Finally, the edge's endpoint's (alter) coordinates are computed as follows: %
//' \deqn{%
//' x_1' = x_1 + \cos(\alpha')\times v_1}{%
//' x1' = x1 + cos(\alpha') * v1
//' }
//' \deqn{%
//' y_1' = y_1 -+ \sin(\alpha')\times v_1 \times\frac{\max y - \min y}{\max x - \min x} }{%
//' y1' = y1 -+ sin(\alpha')*[max(y) - min(y)]/[max(x) - min(x)]
//' }
//' Where \eqn{v_1}{v1} is alter's size in terms of the x-axis, and the sign of
//' the second term in \eqn{y_1'}{y1'} is negative iff \eqn{y_0 < y_1}{y0<y1}.
//' }
//' }
//'
//' The same process (with sign inverted) is applied to the edge starting piont.
//' The resulting values, \eqn{x_1',y_1'}{x1',y1'} can be used with the function
//' \code{\link{arrows}}. This is the workhorse function used in \code{\link{plot_threshold}}.
//'
//' The \code{dev} argument provides a reference to rescale the plot accordingly
//' to the device, and former, considering the size of the margins as well (this
//' can be easily fetched via \code{par("pin")}, plot area in inches).
//'
//' On the other hand, \code{ran} provides a reference for the adjustment
//' according to the range of the data, this is \code{range(x)[2] - range(x)[1]}
//' and \code{range(y)[2] - range(y)[1]} respectively.
//'
//' @keywords misc dplot
//' @examples
//' # --------------------------------------------------------------------------
//' data(medInnovationsDiffNet)
//' library(sna)
//'
//' # Computing coordinates
//' set.seed(79)
//' coords <- sna::gplot(as.matrix(medInnovationsDiffNet$graph[[1]]))
//'
//' # Getting edge coordinates
//' vcex <- rep(1.5, nnodes(medInnovationsDiffNet))
//' ecoords <- edges_coords(
//' medInnovationsDiffNet$graph[[1]],
//' diffnet.toa(medInnovationsDiffNet),
//' x = coords[,1], y = coords[,2],
//' vertex_cex = vcex,
//' dev = par("pin")
//' )
//'
//' ecoords <- as.data.frame(ecoords)
//'
//' # Plotting
//' symbols(coords[,1], coords[,2], circles=vcex,
//' inches=FALSE, xaxs="i", yaxs="i")
//'
//' with(ecoords, arrows(x0,y0,x1,y1, length=.1))
//' @export
// [[Rcpp::export]]
NumericMatrix edges_coords(
const arma::sp_mat & graph,
const arma::colvec & toa,
const arma::colvec & x,
const arma::colvec & y,
const arma::colvec & vertex_cex,
bool undirected=true,
bool no_contemporary=true,
NumericVector dev = NumericVector::create(),
NumericVector ran = NumericVector::create(),
LogicalVector curved = LogicalVector::create()
) {
// The output matrix has the following
// - x0 and y0
// - x1 and y1
// - alpha
std::vector< double > x0;
std::vector< double > y0;
std::vector< double > x1;
std::vector< double > y1;
std::vector< double > alpha;
// Rescaling the vertex sizes
arma::colvec vertex_size(vertex_cex);
// If yexpand is too small, just throw an error
if (ran.length() == 0) {
ran = NumericVector::create(2);
ran[0] = x.max() - x.min();
ran[1] = y.max() - y.min();
}
// Expansion factor for y
double yexpand = 1.0;
if ( ran[1] > 1e-5 ) yexpand = ran[1]/ran[0];
// Adjusting for device size
if (dev.length() == 0)
dev = NumericVector::create(2,1.0);
// Curved?
if (curved.length() == 0)
curved = LogicalVector::create(graph.n_nonzero, true);
yexpand = yexpand * (dev[0]/dev[1]);
for(arma::sp_mat::const_iterator it = graph.begin(); it != graph.end(); ++it) {
int i = it.row();
int j = it.col();
// Checking conditions
if (undirected && (i < j)) continue;
if (no_contemporary && (toa(i)==toa(j)) ) continue;
// Computing angle
double a = atan2((y(j) - y(i))/yexpand, x(j) - x(i));
alpha.push_back(a);
// Adding the xs and the ys
x0.push_back(x.at(i) + cos(a)*vertex_size.at(i));
x1.push_back(x.at(j) - cos(a)*vertex_size.at(j));
// The formula needs an extra help to figure out the ys
y0.push_back(y.at(i) + sin(a)*vertex_size.at(i)*yexpand);
y1.push_back(y.at(j) - sin(a)*vertex_size.at(j)*yexpand);
}
// Building up the output
int e = x0.size();
NumericMatrix out(e,5);
for(int i=0; i<e; ++i) {
out(i,0) = x0[i];
out(i,1) = y0[i];
out(i,2) = x1[i];
out(i,3) = y1[i];
out(i,4) = alpha[i];
}
colnames(out) = CharacterVector::create("x0", "y0", "x1", "y1", "alpha");
return out;
}
// [[Rcpp::export]]
List vertices_coords(
const arma::colvec & x,
const arma::colvec & y,
const arma::colvec & size,
const arma::colvec & nsides,
const arma::colvec & rot,
NumericVector dev = NumericVector::create(),
NumericVector ran = NumericVector::create()
) {
// Checking sizes
if (x.n_rows != y.n_rows) stop("-x- and -y- lengths do not coincide.");
if (x.n_rows != size.n_rows) stop("-x- and -size- lengths do not coincide.");
if (x.n_rows != nsides.n_rows) stop("-x- and -nsides- lengths do not coincide.");
if (x.n_rows != rot.n_rows) stop("-x- and -rot- lengths do not coincide.");
List out(x.n_rows);
// If yexpand is too small, just throw an error
if (ran.length() == 0) {
ran = NumericVector::create(2);
ran[0] = x.max() - x.min();
ran[1] = y.max() - y.min();
}
// Expansion factor for y
double yexpand = 1.0;
if ( ran[1] > 1e-5 ) yexpand = ran[1]/ran[0];
// Adjusting for device size
if (dev.length() == 0)
dev = NumericVector::create(2,1.0);
yexpand = yexpand * (dev[0]/dev[1]);
for (unsigned i=0;i<x.n_rows;++i) {
// Getting inner degrees
double alpha = PI - ((nsides(i) - 2.0)*PI)/nsides(i);
double beta = (PI - 2.0*PI/nsides(i))/2.0;
// Getting step size
double size_adj = 2.0*cos(beta)*size(i);
// Suboutput and first coordinate
arma::mat coords(nsides(i),2);
coords(0,0) = x(i) + size(i)*cos(beta + PI + rot(i));
coords(0,1) = y(i) + size(i)*sin(beta + PI + rot(i))*yexpand;
double ALPHA = rot(i);
for (int j=1; j<nsides(i); ++j) {
coords(j,0) = coords(j-1,0) + size_adj*cos(ALPHA);
coords(j,1) = coords(j-1,1) + size_adj*sin(ALPHA)*yexpand;
ALPHA += alpha;
}
// Assigning element
out[i] = coords;
}
return out;
}
//--------------------------------------------------------------------------------------------------
double polynomial_kernel( const arma::colvec& x, const arma::colvec& y,
const double& alpha, const double& beta, const double& n ) {
return pow( as_scalar( alpha * x.t() * y ) + beta, n );
}