void deriveEquiFreqAndGtrParamsForReversibleRM(matrix_t const & Q, vector_t & equiFreq, vector_t & gtrParams)
{
equiFreq = deriveEquiFreqForReversibleRM(Q);
unsigned n = equiFreq.size();
// check
for (unsigned i = 0; i < n; i++)
assert(equiFreq[i] > 0);
matrix_t pm = Q; // parameter matrix
for (unsigned i = 0; i < n; i++)
for (unsigned j = 0; j < n; j++)
pm(i, j) = Q(i, j) / equiFreq[j];
// check grtParams size
unsigned paramCount = (n * (n - 1) / 2);
if (gtrParams.size() != paramCount)
gtrParams.resize(paramCount);
// reverse of gtrRateParametersToMatrix
unsigned idx = 0;
for (unsigned i = 0; i < n; i++)
for (unsigned j = i + 1; j < n; j++) {
gtrParams[idx] = pm(i, j);
idx++;
}
}
void lfit(vector_t &x, vector_t &y, vector_t &sig, vector_t &a,
vector<bool> &ia, matrix_t &covar, double &chisq, matrix_t & X)
{
int i,j,k,l,m,mfit=0;
double ym,wt,sum,sig2i;
int ndat=x.size();
int ma=a.size();
vector_t afunc(ma);
matrix_t beta;
sizeMatrix(beta,ma,1);
for (j=0;j<ma;j++)
if (ia[j]) mfit++;
if (mfit == 0) error("lfit: no parameters to be fitted");
for (j=0;j<mfit;j++) {
for (k=0;k<mfit;k++) covar[j][k]=0.0;
beta[j][0]=0.0;
}
for (i=0;i<ndat;i++) {
afunc = X[i];
ym=y[i];
if (mfit < ma) {
for (j=0;j<ma;j++)
if (!ia[j]) ym -= a[j]*afunc[j];
}
sig2i=1.0/SQR(sig[i]);
for (j=0,l=0;l<ma;l++) {
if (ia[l]) {
wt=afunc[l]*sig2i;
for (k=0,m=0;m<=l;m++)
if (ia[m]) covar[j][k++] += wt*afunc[m];
beta[j++][0] += ym*wt;
}
}
}
for (j=1;j<mfit;j++)
for (k=0;k<j;k++)
covar[k][j]=covar[j][k];
vector<vector<double> > temp;
sizeMatrix(temp,mfit,mfit);
for (j=0;j<mfit;j++)
for (k=0;k<mfit;k++)
temp[j][k]=covar[j][k];
gaussj(temp,beta);
for (j=0;j<mfit;j++)
for (k=0;k<mfit;k++)
covar[j][k]=temp[j][k];
for (j=0,l=0;l<ma;l++)
if (ia[l]) a[l]=beta[j++][0];
chisq=0.0;
for (i=0;i<ndat;i++) {
afunc = X[i];
sum=0.0;
for (j=0;j<ma;j++) sum += a[j]*afunc[j];
chisq += SQR((y[i]-sum)/sig[i]);
}
covsrt(covar,ia,mfit);
}
vector_t next_vec( const vector_t ¤t, const vector_t &grad,
const vector_t &lapl ) {
vector_t next( current.size() );
// std::cout << "next_vec" << std::endl;
// std::cout << current.size() << " " << grad.size() << " "
// << lapl.size() << std::endl;
for ( unsigned int i = 0; i < current.size(); ++i ) {
next[i] = current[i] - grad[i] / lapl[i]; }
return next; }
static void CheckVector( const vector_t& cv, size_t expected_size, size_t old_size ) {
ASSERT( cv.capacity()>=expected_size, NULL );
ASSERT( cv.size()==expected_size, NULL );
ASSERT( cv.empty()==(expected_size==0), NULL );
for( int j=0; j<int(expected_size); ++j ) {
if( cv[j].bar()!=~j )
REPORT("ERROR on line %d for old_size=%ld expected_size=%ld j=%d\n",__LINE__,long(old_size),long(expected_size),j);
}
}
bool ModelUtil::EarlyStop(vector_t val_losses, size_t patience, float delta) {
// Check for edge cases
PELOTON_ASSERT(patience > 1);
PELOTON_ASSERT(delta > 0);
if (val_losses.size() < patience) return false;
float cur_loss = val_losses[val_losses.size() - 1];
float pat_loss = val_losses[val_losses.size() - patience];
// Loss should have at least dropped by delta at this point
return (pat_loss - cur_loss) < delta;
}
quat_t MeCtQuickDraw::rotation_to_target( vector_t l_forward_dir, vector_t w_target_pos, joint_state_t* state_p ) {
vector_t l_target_dir_n = ( -state_p->parent_rot ) * ( w_target_pos - state_p->world_pos ).normal();
gwiz::float_t rad = safe_acos( l_forward_dir.dot( l_target_dir_n ) );
gwiz::float_t angle = deg( rad );
vector_t axis = l_forward_dir.cross( l_target_dir_n ).normal();
return( quat_t( angle, axis ) );
}
void rl_nac::solve_for_grad( vector_t& guess ) {
matrix_t copyA;
//vector_t guess;
LapackInfo info(0);
copyA = A;
guess = b;
// Regularize this: A = A + REGULARIZER*eye(N)
int cnt = A.GetM();
for ( int i=0; i<cnt; i++ ) {
copyA(i,i) = copyA(i,i) + REGULARIZER;
}
#ifdef DEBUG
printf("=====================================\n");
printf("Solving for gradient:\n");
copyA.Print();
guess.Print();
printf("=====================================\n");
#endif
vector_t tau;
GetQR( copyA, tau, info );
if ( info.GetInfo() != 0 ) {
fprintf( stderr, "Error in QR decomposition!\n" );
for ( int a=0; a<act_cnt; a++ ) {
(*acts)[a]->natural_grad.Fill(0);
}
guess.Fill( 0 );
return;
}
SolveQR( copyA, tau, guess, info );
if ( info.GetInfo() != 0 ) {
fprintf( stderr, "Error solving linear system!\n" );
for ( int a=0; a<act_cnt; a++ ) {
(*acts)[a]->natural_grad.Fill(0);
}
guess.Fill( 0 );
return;
}
// results are stored in guess
#ifdef DEBUG
printf("Final answer:\n");
guess.Print();
#endif
return;
}
scalar_t vgrad(const vector_t& x, vector_t* gx) const override
{
m_idata = map_tensor(x.data(), m_idata.dims());
m_op.output(m_idata, m_wdata, m_bdata, m_odata);
if (gx)
{
gx->resize(x.size());
auto idata = map_tensor(gx->data(), m_idata.dims());
m_op.ginput(idata, m_wdata, m_bdata, m_odata);
}
return m_odata.array().square().sum() / 2;
}
void quaternion_t::compute_rotation(const vector_t& v1, const vector_t& v2)
{
PRX_ASSERT(v1.get_dim() == v2.get_dim());
vector_t v(3);
double w;
v.cross_product(v1,v2);
w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
set(v[0],v[1],v[2],w);
normalize();
}
static std::uint32_t to(state_t &state, const vector_t &val)
{
::lua_createtable(state, (int)val.size(), (int)val.size());
std::uint32_t i = 1;
std::for_each(val.cbegin(), val.cend(),
[&state, &i](const T &t)
{
convertion_t<T>::to(state, t);
::lua_rawseti(state, -2, i++);
});
return 1;
}
scalar_t vgrad(const vector_t& x, vector_t* gx) const override
{
m_wdata = map_matrix(x.data(), m_wdata.rows(), m_wdata.cols());
m_bdata = map_vector(x.data() + m_wdata.size(), m_bdata.size());
m_op.output(m_idata, m_wdata, m_bdata, m_odata);
if (gx)
{
gx->resize(x.size());
auto wdata = map_matrix(gx->data(), m_wdata.rows(), m_wdata.cols());
auto bdata = map_vector(gx->data() + m_wdata.size(), m_bdata.size());
m_op.gparam(m_idata, wdata, bdata, m_odata);
}
return m_odata.array().square().sum() / 2;
}
discrete_task(discrete_device &pdev)
: task_group(0), m_device(pdev), m_threadid(-1)
{
source_list.clear();
step_list.clear();
m_buffers.clear();
}
void pvt_base::check_gas_common (const vector_t &pressure, const vector_t &fvf, const vector_t &visc)
{
for (t_long i = 1, cnt = (t_long)pressure.size (); i < cnt; ++i)
{
if (pressure[i] - pressure[i - 1] < EPS_DIFF)
{
throw bs_exception ("", "pressure curve should be monotonically increasing function");
}
if (fvf[i] - fvf[i - 1] >= 0)
{
BOSOUT (section::pvt, level::critical) << "gas: fvf" << bs_end;
for (t_long j = 0; j < cnt; ++j)
{
BOSOUT (section::pvt, level::critical) << fvf[j] << bs_end;
}
throw bs_exception ("", "FVF curve should be monotonically decreasing function");
}
if (visc[i] - visc[i - 1] <= 0)
{
BOSOUT (section::pvt, level::critical) << "gas: visc" << bs_end;
for (t_long j = 0; j < cnt; ++j)
{
BOSOUT (section::pvt, level::critical) << visc[j] << bs_end;
}
throw bs_exception ("", "Viscosity curve should be monotonically increasing function");
}
}
}
void
pvt_base::check_oil_common (const vector_t &pressure, const vector_t &fvf, const vector_t &visc)
{
for (t_long i = 0, cnt = (t_long)pressure.size (); i < cnt; ++i)
{
if (pressure[i] < 0)
{
// TODO: LOG
BS_ASSERT (false) (pressure[i]);
throw bs_exception ("", "pressure should be greater than 0");
}
if (fvf[i] < 0)
{
// TODO:LOG
BS_ASSERT (false) (fvf[i]);
throw bs_exception ("", "fvf should be greater than 0");
}
if (visc[i] < 0)
{
// TODO: LOG
BS_ASSERT (false) (visc[i]);
throw bs_exception ("", "viscosity should be greater than 0");
}
}
}
// QR Factorization of a MxN General Matrix A.
// a (IN/OUT - matrix(M,N)) On entry, the coefficient matrix A. On exit , the upper triangle and diagonal is the min(M,N) by N upper triangular matrix R. The lower triangle, together with the tau vector, is the orthogonal matrix Q as a product of min(M,N) elementary reflectors.
// tau (OUT - vector (min(M,N))) Vector of the same numerical type as A. The scalar factors of the elementary reflectors.
// info (OUT - int)
// 0 : function completed normally
// < 0 : The ith argument, where i = abs(return value) had an illegal value.
int geqrf (matrix_t& a, vector_t& tau)
{
int _m = int(a.size1());
int _n = int(a.size2());
int _lda = int(a.size1());
int _info;
// make_sure tau's size is greater than or equal to min(m,n)
if (int(tau.size()) < (_n<_m ? _n : _m) )
return -104;
int ldwork = _n*_n;
vector_t dwork(ldwork);
rawLAPACK::geqrf (_m, _n, a.data().begin(), _lda, tau.data().begin(), dwork.data().begin(), ldwork, _info);
return _info;
}
bool geometry_t::contains_point(const vector_t& v, double buffer)
{
double squared_rad;
vector_t n;
switch(type)
{
case PRX_SPHERE://sphere
squared_rad = info[0]*info[0];
if(v.squared_norm() < squared_rad+buffer)
return true;
break;
case PRX_BOX://box
if(v[0]>-.5*info[0]-buffer && v[0]<.5*info[0]+buffer &&
v[1]>-.5*info[1]-buffer && v[1]<.5*info[1]+buffer &&
v[2]>-.5*info[2]-buffer && v[2]<.5*info[2]+buffer)
{
return true;
}
break;
case PRX_CONE:
PRX_WARN_S("Cannot check point containment in cone");
break;
case PRX_CYLINDER://cylinder
case PRX_OPEN_CYLINDER:
n = v;
n[2] = 0;
squared_rad = info[0]*info[0]+buffer;
if(v.squared_norm() < squared_rad &&
v[2]>-.5*info[1]-buffer && v[2]<.5*info[1]+buffer )
return true;
break;
case PRX_CAPSULE:
PRX_WARN_S("Cannot check point containment in capsule");
break;
case PRX_MESH:
// PRX_WARN_S(trimesh.min_x<<" "<<trimesh.max_x<<" "<<trimesh.min_y<<" "<<trimesh.max_y<<" "<<trimesh.min_z<<" "<<trimesh.max_z);
if( trimesh.min_x - buffer < v[0] && trimesh.max_x + buffer > v[0]
&& trimesh.min_y - buffer < v[1] && trimesh.max_y + buffer > v[1]
&& trimesh.min_z - buffer < v[2] && trimesh.max_z + buffer > v[2])
return true;
break;
default:
PRX_WARN_S("Cannot check point containment in geometry");
}
return false;
}
void quaternion_t::set( const vector_t& v)
{
PRX_ASSERT(v.get_dim() == 4);
q[0] = v[0];
q[1] = v[1];
q[2] = v[2];
q[3] = v[3];
}
void quaternion_t::get( vector_t& v) const
{
PRX_ASSERT(v.get_dim() == 4);
v[0] = q[0];
v[1] = q[1];
v[2] = q[2];
v[3] = q[3];
}
unsigned countNonZeroEntries(vector_t const & v)
{
unsigned n = 0;
for (unsigned i = 0; i < v.size(); i++)
if ( v(i) )
n++;
return n;
}
boost::shared_ptr<BaseTRRateMatrix> TRRateMatrixDispatcher(string const & substModel, vector_t const & rateParameters, vector_t const & equiFreq, StateMap const & alphabet)
{
boost::shared_ptr<BaseTRRateMatrix> rmPtr;
if (substModel == "GTR")
rmPtr = boost::shared_ptr<GTRRateMatrix>( new GTRRateMatrix(alphabet) );
// define other substitution models here ...
else
errorAbort("substitution model '" + substModel + "' not defined. If newly implemented, add to TRRateMatrixDispatcher.");
if (equiFreq.size() > 0) // not defined, stick with default
rmPtr->resetEquiFreqs(equiFreq);
if (rateParameters.size() > 0) // not defined, stick with default
rmPtr->resetRateParameters(rateParameters);
return rmPtr;
}
void build_sah(vector_t<PrimRef>& prims, isa::PrimInfo& pinfo)
{
size_t N = pinfo.size();
/* fast allocator that supports thread local operation */
FastAllocator allocator;
for (size_t i=0; i<2; i++)
{
std::cout << "iteration " << i << ": building BVH over " << N << " primitives, " << std::flush;
double t0 = getSeconds();
allocator.reset();
Node* root;
isa::BVHBuilderBinnedSAH::build<Node*>(
root,
/* thread local allocator for fast allocations */
[&] () -> FastAllocator::ThreadLocal* {
return allocator.threadLocal();
},
/* lambda function that creates BVH nodes */
[&](const isa::BVHBuilderBinnedSAH::BuildRecord& current, isa::BVHBuilderBinnedSAH::BuildRecord* children, const size_t N, FastAllocator::ThreadLocal* alloc) -> int
{
assert(N <= 2);
InnerNode* node = new (alloc->malloc(sizeof(InnerNode))) InnerNode;
for (size_t i=0; i<N; i++) {
node->bounds[i] = children[i].pinfo.geomBounds;
children[i].parent = (size_t*) &node->children[i];
}
*current.parent = (size_t) node;
return 0;
},
/* lambda function that creates BVH leaves */
[&](const isa::BVHBuilderBinnedSAH::BuildRecord& current, FastAllocator::ThreadLocal* alloc) -> int
{
assert(current.prims.size() == 1);
Node* node = new (alloc->malloc(sizeof(LeafNode))) LeafNode(prims[current.prims.begin()].ID(),prims[current.prims.begin()].bounds());
*current.parent = (size_t) node;
return 0;
},
/* progress monitor function */
[&] (size_t dn) {
// throw an exception here to cancel the build operation
},
prims.data(),pinfo,2,1024,1,1,1,1.0f,1.0f);
double t1 = getSeconds();
std::cout << 1000.0f*(t1-t0) << "ms, " << 1E-6*double(N)/(t1-t0) << " Mprims/s, sah = " << root->sah() << " [DONE]" << std::endl;
}
}
UG_API bool
ContainsPoint(const EdgeVertices* e, const vector_t& p, TAAPos aaPos)
{
number center = (aaPos[e->vertex(0)].x() + aaPos[e->vertex(1)].x()) / 2.;
number rad = fabs(aaPos[e->vertex(1)].x() - aaPos[e->vertex(0)].x()) / 2.;
if(fabs(p.x() - center) <= rad)
return true;
return false;
}
double vector_t::get_angle_between(const vector_t& target ) const
{
double dot_prod = this->dot_product(target);
double m1 = this->norm();
double m2 = target.norm();
double product = dot_prod/(m1*m2);
if (product > 1)
return 0;
else if (product < -1)
return PRX_PI;
return acos(product);
}
void quaternion_t::compute_rotation(const vector_t& v1, const vector_t& v2)
{
PRX_ASSERT(v1.get_dim() == v2.get_dim());
vector_t v(3);
double w;
// v.cross_product(v1,v2);
//
// if ( fabs(v1.norm()-1) <= PRX_ZERO_CHECK && fabs(v2.norm()-1) <= PRX_ZERO_CHECK ) {
// w = 1 + v1.dot_product(v2);
// } else {
// w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
// }
//
// set(v[0],v[1],v[2],w);
v.cross_product(v1,v2);
w = sqrt(v1.squared_norm() * v2.squared_norm()) + v1.dot_product(v2);
set(v[0],v[1],v[2],w);
// PRX_DEBUG_S("q: " << *this);
normalize();
// PRX_DEBUG_S("AFTER q: " << *this);
}
void quaternion_t::set( const vector_t& axis, double angle)
{
PRX_ASSERT(axis.get_dim() == 3);
if (fabs(angle) < PRX_ZERO_CHECK)
{
clear();
return;
}
angle *= 0.5;
double sinAngle = sin(angle);
q[0] = axis[0] * sinAngle;
q[1] = axis[1] * sinAngle;
q[2] = axis[2] * sinAngle;
q[3] = cos(angle);
}
void SCL::get_rhs(fe_func_t& v_h, vector_t& rhs)
{
update_edge_cache(v_h);
double alpha = 1.0;
rhs.reinit(fem_space->n_dof());
fe_space_t::DGElementIterator
the_dgele = fem_space->beginDGElement(),
end_dgele = fem_space->endDGElement();
for (;the_dgele != end_dgele;++ the_dgele) {
GET_EDGE_CACHE(*the_dgele);
const element_t& neigh0 = the_dgele->neighbourElement(0);
const std::vector<int>& ele_dof0 = neigh0.dof();
if (the_dgele->p_neighbourElement(1) != NULL) { // 内部边
const element_t& neigh1 = the_dgele->neighbourElement(1);
const std::vector<int>& ele_dof1 = neigh1.dof();
for (int l = 0;l < n_q_pnt;l ++) {
double a = Jxw[l]*flux(q_pnt[l], u_h_val0[l], u_h_val1[l], un[l]);
rhs(ele_dof0[0]) -= a;
rhs(ele_dof1[0]) += a;
}
} else { // 分区之间的边界和区域的物理边界上的边
for (int l = 0;l < n_q_pnt;l ++) {
double a = Jxw[l]*flux(q_pnt[l], u_h_val0[l], u_h_val1[l], un[l]);
rhs(ele_dof0[0]) -= a;
}
}
}
fe_space_t::ElementIterator
the_ele = fem_space->beginElement(),
end_ele = fem_space->endElement();
for (;the_ele != end_ele;++ the_ele) {
const std::vector<int>& ele_dof = the_ele->dof();
rhs(ele_dof[0]) /= element_cache[the_ele->index()].vol;
}
}
const T& operator[]( size_type index )
{
return data_.at( index );
}
// changing (increasing) the capacity:
void reserve(const size_type r) {
stack.reserve(r);
}
size_type size() const
{
return data_.size();
}
// "capacity" refers to the size possible without (internal) re-allocation:
size_type capacity() const {
return stack.capacity();
}
