TEST_F(ch_chanFixture, belowIsAbove){
Point_2 p(0,1);
Point_2 vi(0,0);
Point_2 vj(1,0);
bool actual = below(p,vi,vj);
EXPECT_FALSE(actual);
}
TEST_F(ch_chanFixture, belowIsBelow){
Point_2 p(0,-1);
Point_2 vi(0,0);
Point_2 vj(1,0);
bool actual = below(p,vi,vj);
EXPECT_TRUE(actual);
}
TEST_F(ch_chanFixture, aboveIsBelow){
Point_2 p(0,-1);
Point_2 vi(0,0);
Point_2 vj(1,0);
bool actual = above(p,vi,vj);
EXPECT_FALSE(actual);
}
TEST_F(ch_chanFixture, aboveIsAbove){
Point_2 p(0,1);
Point_2 vi(0,0);
Point_2 vj(1,0);
bool actual = above(p,vi,vj);
EXPECT_TRUE(actual);
}
MatrixXd Neighborhood::outerProduct(int i, int j){
VectorXd vi(nfeat);
VectorXd vj(nfeat);
dataPointToVector((*sd).getDataPoint(i), vi);
dataPointToVector((*sd).getDataPoint(j), vj);
return (vi - vj) * (vi - vj).transpose();
}
double Neighborhood::mdist(int i, int j, MatrixXd& M){
VectorXd vi(nfeat);
VectorXd vj(nfeat);
dataPointToVector((*sd).getDataPoint(i), vi);
dataPointToVector((*sd).getDataPoint(j), vj);
return (vi - vj).transpose() * M * (vi - vj);
}
void KernelNeighborhood::generateTestKernelMatrix(){
K_test.resize(sd->ninst, sd_test->ninst);
VectorXd vi(nfeat);
VectorXd vj(nfeat);
for(int i = 0; i < sd->ninst; i++){
sd->getVector(i, vi);
for(int j = 0; j < sd_test->ninst; j++){
sd_test->getVector(j, vj);
K_test(i, j) = kernel_rbf(vi, vj);
}
}
}
double LCOrbits::ArmVelocity(int em, int rec, double trec)
{
LCVector ri(MT), rj(MT), vi(MT), vj(MT), rij(MT), nij(MT), n(MT);
ri = position(rec, trec);
vi = velocity(rec, trec);
rj = position(em, trec);
vj = velocity(em, trec);
rij = rj-ri;
nij = rij.unit();
n = nij * (-1.);
return(vj*n-vi*n);
}
void KernelNeighborhood::generateKernelMatrix(){
K.resize(ninst, ninst);
VectorXd vi(nfeat);
VectorXd vj(nfeat);
for(int i = 0; i < ninst; i++){
(*sd).getVector(i, vi);
for(int j = 0; j < ninst; j++){
if(i==j)
K(i, j) = 1.0;
else if(i>j)
K(i, j) = K(j, i);
else{
(*sd).getVector(j, vj);
K(i, j) = kernel_rbf(vi, vj);
}
}
}
}
void MxPropSlim::compute_target_placement(edge_info *info)
{
MxVertexID i=info->v1, j=info->v2;
const MxQuadric &Qi=quadric(i), &Qj=quadric(j);
MxQuadric Q=Qi; Q+=Qj;
double e_min;
if( placement_policy==MX_PLACE_OPTIMAL && Q.optimize(info->target)){
e_min = Q(info->target);
}else{
// Fall back only on endpoints
MxVector vi(dim());
MxVector vj(dim());
MxVector best(dim());
pack_to_vector(i, vi);
pack_to_vector(j, vj);
double ei=Q(vi), ej=Q(vj);
if( ei<ej ) { e_min = ei; best = vi; }
else { e_min = ej; best = vj; swap(info->v1,info->v2);}
if( placement_policy>=MX_PLACE_ENDORMID ){
MxVector mid(dim());
mid = vi;
mid +=vj;
mid /=2.f;
double e_mid= Q(mid);
if( e_mid < e_min ) { e_min = e_mid; best = mid; }
}
info->target = best;
}
if( weighting_policy == MX_WEIGHT_AREA_AVG )
e_min /= Q.area();
info->heap_key(float(-e_min));
}
OBAPI void SetTorsion(double *c, unsigned int ref[4], double setang, std::vector<int> atoms)
{
unsigned int cidx[4];
cidx[0] = (ref[0] - 1) * 3;
cidx[1] = (ref[1] - 1) * 3;
cidx[2] = (ref[2] - 1) * 3;
cidx[3] = (ref[3] - 1) * 3;
const Eigen::Vector3d va( c[cidx[0]], c[cidx[0]+1], c[cidx[0]+2] );
const Eigen::Vector3d vb( c[cidx[1]], c[cidx[1]+1], c[cidx[1]+2] );
const Eigen::Vector3d vc( c[cidx[2]], c[cidx[2]+1], c[cidx[2]+2] );
const Eigen::Vector3d vd( c[cidx[3]], c[cidx[3]+1], c[cidx[3]+2] );
// calculate the rotation angle
const double ang = setang - DEG_TO_RAD * VectorTorsion(va, vb, vc, vd);
// the angles are the same...
if (fabs(ang) < 1e-5)
return;
// setup the rotation matrix
const Eigen::Vector3d bc = (vb - vc).normalized();
Eigen::Quaternion<double> m;
m = Eigen::AngleAxis<double>(-ang, bc);
// apply the rotation
int j;
for (int i = 0; i < atoms.size(); ++i) {
j = (atoms[i] - 1) * 3;
Eigen::Vector3d vj( c[j], c[j+1], c[j+2] );
vj -= vb; // translate so b is at origin
vj = m.toRotationMatrix() * vj;
vj += vb; // translate back
c[j] = vj.x();
c[j+1] = vj.y();
c[j+2] = vj.z();
}
}
CoupleItem BppSolver::ryanFosterCouple(BranchNode& node)
{
// get var values
auto& Xs = BPmodel.getPrimalValues();
#ifdef DBG_OUTPUT
for (int i = 0; i < BPVars.getSize(); i++)
std::cout << Xs[i] << " ";
std::cout << std::endl;
#endif
std::vector<std::size_t> vec_shuffled(Xs.getSize());
std::iota(vec_shuffled.begin(), vec_shuffled.end(), 0);
// shuffle
auto engine = std::default_random_engine(std::random_device{}());
std::shuffle(vec_shuffled.begin(), vec_shuffled.end(), engine);
std::list<std::size_t> vi(vec_shuffled.begin(), vec_shuffled.end());
std::shuffle(vec_shuffled.begin(), vec_shuffled.end(), engine);
std::list<std::size_t> vj(vec_shuffled.begin(), vec_shuffled.end());
for (auto it = vi.begin(); it != vi.end(); it++)
{
int i = *it;
auto jt = vj.begin();
while (jt != vj.end())
{
int j = *jt;
if (i == j)
jt = vj.erase(jt);
else
{
IloExpr::LinearIterator it_i = BPmodel.getLinearCoefIterator(i);
IloExpr::LinearIterator it_j = BPmodel.getLinearCoefIterator(j);
real sumVars = 0.0;
for (; ; )
{
// get index of the variables via thier names
std::string name_i(it_i.getVar().getName());
std::string name_j(it_j.getVar().getName());
// si and sj are the column index of the iterators
int si = std::stoi(name_i.substr(1));
int sj = std::stoi(name_j.substr(1));
// booleans for validity of the columns
bool validColumn_it = true;
// i and j are in the same bin
if (si == sj)
{
for (auto cols : node.incumbentInvalidColumns) {
if (!(validColumn_it = (cols->find(sj) == cols->end())))
break;
}
if (validColumn_it)
validColumn_it = (node.invalidColumns->find(si) == node.invalidColumns->end());
if (validColumn_it && (Xs[si] > 0.0))
sumVars += Xs[si];
}
// some requirement to increment the iterator
// since they itarate only over non zero coefs
if (si <= sj)
++it_i;
++it_j;
// stop condition
if (!it_i.ok() || !it_j.ok())
break;
}
real _dumy;
real frac = modf(sumVars, &_dumy);
if (frac >= 0.0001) {
return std::make_pair(i, j);
}
++jt;
}
}
}
}
void KX_NavMeshObject::DrawNavMesh(NavMeshRenderMode renderMode)
{
if (!m_navMesh)
return;
MT_Vector3 color(0.f, 0.f, 0.f);
switch (renderMode)
{
case RM_POLYS :
case RM_WALLS :
for (int pi=0; pi<m_navMesh->getPolyCount(); pi++)
{
const dtStatPoly* poly = m_navMesh->getPoly(pi);
for (int i = 0, j = (int)poly->nv-1; i < (int)poly->nv; j = i++)
{
if (poly->n[j] && renderMode==RM_WALLS)
continue;
const float* vif = m_navMesh->getVertex(poly->v[i]);
const float* vjf = m_navMesh->getVertex(poly->v[j]);
MT_Point3 vi(vif[0], vif[2], vif[1]);
MT_Point3 vj(vjf[0], vjf[2], vjf[1]);
vi = TransformToWorldCoords(vi);
vj = TransformToWorldCoords(vj);
KX_RasterizerDrawDebugLine(vi, vj, color);
}
}
break;
case RM_TRIS :
for (int i = 0; i < m_navMesh->getPolyDetailCount(); ++i)
{
const dtStatPoly* p = m_navMesh->getPoly(i);
const dtStatPolyDetail* pd = m_navMesh->getPolyDetail(i);
for (int j = 0; j < pd->ntris; ++j)
{
const unsigned char* t = m_navMesh->getDetailTri(pd->tbase+j);
MT_Point3 tri[3];
for (int k = 0; k < 3; ++k)
{
const float* v;
if (t[k] < p->nv)
v = m_navMesh->getVertex(p->v[t[k]]);
else
v = m_navMesh->getDetailVertex(pd->vbase+(t[k]-p->nv));
float pos[3];
rcVcopy(pos, v);
flipAxes(pos);
tri[k].setValue(pos);
}
for (int k=0; k<3; k++)
tri[k] = TransformToWorldCoords(tri[k]);
for (int k=0; k<3; k++)
KX_RasterizerDrawDebugLine(tri[k], tri[(k+1)%3], color);
}
}
break;
default:
/* pass */
break;
}
}
double LCOrbits::ArmCompute(int em, int rec, double trec)
{
double tA(0.), tij;
if ((em<1)||(em>NARMMAX)||((rec<1)&&(rec>NARMMAX))){
std::cerr << "ERROR in LCOrbits::ArmCompute : Spacecraft number must be in [0," << NARMMAX << "] ! " << Endl;
throw std::invalid_argument ("ERROR in LCOrbits::ArmCompute : Spacecraft number must be in [0,NARMMAX] ! ");
}
/*
if(OrderArm != -10)
OrderArm = order_default;
if ((OrderArm<-2)||(OrderArm>2))
throw std::invalid_argument ("ERROR in LCOrbits::ArmCompute : Order must be 0 (for 0), 1 (for 1/2), 2 (for 1) or -1 (for analytical MLDC eccentric formulation)");
*/
if(OrderArm >= 0){
LCVector ri(MT), rj(MT), vi(MT), vj(MT), rij(MT), nij(MT), n(MT);
/*! **** Read positions and velocities and compute related quantities
* Index i refers to receiver and j to emitter
* \f{eqnarray*}{
* \vec{r}_{ij} & = & \vec{r}_j - \vec{r}_i = \vec{r}_{em} - \vec{r}_{rec} \\
* \vec{n}_{ij} & = & \vec{r}_{ij} / | \vec{r}_{ij} | \\
* \hat{n} & = & - \vec{n}_{ij} \\
* t_{ij} & = & - | \vec{r}_{ij} | / c = - | \vec{r}_{em} - \vec{r}_{rec} | / c
* \f}
*/
ri = position(rec, trec);
vi = velocity(rec, trec);
vi = vi/LC::c_SI;
rj = position(em, trec);
vj = velocity(em, trec);
vj = vj/LC::c_SI;
/*
ri.p[0] = 5.e9;
ri.p[1] = 0.;
ri.p[2] = 0.;
rj.p[0] = -1.e-5;
rj.p[1] = 0.;
rj.p[2] = 0.;
rij = rj-ri;
*/
rij = rj-ri;
nij = rij.unit();
n = nij*(-1.);
tij = rij.norm()/LC::c_SI;
tij = -tij;
/*
MT->o->precision(16);
Cout << "em = " << em << " -> rec = " << rec << " : " << rij.norm() << " ==> " << tij << Endl;
for(int i=0; i<3; i++)
Cout << "\t\t" << ri.p[i] << "\t\t" << rj.p[i] << "\t\t" << rij.p[i] << Endl;
*/
if (OrderArm>=0)
tA += ArmOrderContrib(0, tij, ri, rj, vi, vj, rij, nij, n);
if (OrderArm>=1)
tA += ArmOrderContrib(1, tij, ri, rj, vi, vj, rij, nij, n);
if (OrderArm>=2)
tA += ArmOrderContrib(2, tij, ri, rj, vi, vj, rij, nij, n);
return (tA);
}else{
return( ArmSpecific(em, rec, trec) );
}
}
void CalculateAngleDerivativesE2(vtkPolyData* mesh, const arma::sp_mat& edge_lengths, const arma::vec& radii) {
//calculate the power center for each triangle as follows
//for 2 circles centered at c1 and c2 solve for p:
//|p-c1|^2 - r1^2 = |p-c2|^2 - r2^2 -- this is power line
//add one more condition for the third circle to get one point
// Funny part: the paper says that the circle of radius sqrt(|c1-p|^2 - r1^2)
// is perpendicular to all three circles. But this works only when the 3 circles do not intersect
const int npts = mesh->GetNumberOfPoints();
const int ncells = mesh->GetNumberOfCells();
vtkSmartPointer<vtkIdList> cellIdList = vtkSmartPointer<vtkIdList>::New();
vtkSmartPointer<vtkIdList> pointIdList = vtkSmartPointer<vtkIdList>::New();
//for every face calculate gamma_ijk
arma::vec gamma_percell(ncells);
for (vtkIdType i = 0; i < ncells; i++) {
pointIdList->Reset();
mesh->GetCellPoints(cellIdList->GetId(i), pointIdList);
const int ind_i = pointIdList->GetId(0);
const int ind_j = pointIdList->GetId(1);
const int ind_k = pointIdList->GetId(2);
const double ri = radii[ind_i];
const double rj = radii[ind_j];
const double rk = radii[ind_k];
arma::vec ci(3), cj(3), ck(3);
mesh->GetPoint(ind_i, ci.memptr());
mesh->GetPoint(ind_j, cj.memptr());
mesh->GetPoint(ind_k, ck.memptr());
//create local 2D coordinates, and map vertices
arma::vec vji = cj - ci;
arma::vec v = ck - ci;
arma::vec n = arma::cross(vji, v); //normal
arma::vec vki = arma::cross(n, vji); //creates local y' axis
const double lenX = arma::norm(vji,2);
const double lenY = arma::norm(vki,2);
vji /= lenX; //normalize axes
vki /= lenY;
//2D coordinates
arma::vec vi(3,0); //(0,0)
arma::vec vj(3);
arma::vec vk(3);
vj(0) = lenX;
vj(1) = 0.0;
vk(0) = arma::dot(v, vji);
vk(1) = arma::dot(v, vki);
//form matrices for system of linear equations and solve
//A = [2*(c2-c1)'; 2*(c2-c3)'];
//B = [ c2'*c2 - c1'*c1 + r1^2 - r2^2; c2'*c2 - c3'*c3 + r3^2 - r2^2];
//P = A\B;
arma::mat A(2,2);
arma::vec B(2);
A.insert_rows( 0, 2.0*(cj-ci) );
A.insert_rows( 1, 2.0*(cj-ck) );
B(0) = arma::dot(cj, cj) - arma::dot(ci, ci) + ri*ri - rj*rj;
B(1) = arma::dot(cj, cj) - arma::dot(ck, ck) + rk*rk - rj*rj;
arma::vec P = arma::solve( A, B );
//calculate distance from P to all the edges: hi, hj, hk
//http://mathworld.wolfram.com/Point-LineDistance2-Dimensional.html
const double hi = arma::norm( arma::cross(cj-ck, ck-P), 2 ) / arma::norm(cj-ck, 2);
const double hj = arma::norm( arma::cross(ci-ck, ck-P), 2 ) / arma::norm(ci-ck, 2);
const double hk = arma::norm( arma::cross(cj-ci, ci-P), 2 ) / arma::norm(cj-ci, 2);
const double li = edge_lengths(ind_j, ind_k);
const double lj = edge_lengths(ind_i, ind_k);
const double lk = edge_lengths(ind_i, ind_j);
const double dTi_duj = hk/lk;
const double dTj_duk = hi/li;
const double dTk_dui = hj/lj;
const double dTi_dui = - dTi_duj - dTk_dui;
const double dTj_duj = - dTj_duk - dTi_duj;
const double dTk_duk = - dTj_duk - dTk_dui;
}
}
Twist UndirectedTreeLink::vj(LinkMap::const_iterator adjacent_iterator, const double& q,const double& qdot) const
{
int adjacent_index = this->globalIterator2localIndex(adjacent_iterator);
return vj(adjacent_index,q,qdot);
}
