void Poly::draw(int gl_type, double z, bool randomized) const
{
Vector2d v;
uint count = vertices.size();
if (!closed && gl_type == GL_LINE_LOOP) {
gl_type = GL_LINES;
count--;
}
glBegin(gl_type);
for (uint i=0; i < count; i++){
v = getVertexCircular(i);
if (randomized) v = random_displaced(v);
glVertex3f(v.x(),v.y(),z);
if ( gl_type == GL_LINES ) {
Vector2d vn = getVertexCircular(i+1);
if (randomized) vn = random_displaced(vn);
glVertex3f(vn.x(),vn.y(),z);
}
}
glEnd();
// if (hole) {
// glBegin(GL_LINES);
// for (uint i=0; i < count; i++){
// glVertex3d(center.x(),center.y(),z);
// Vector2d vn = vertices[i];
// if (randomized) vn = random_displaced(vn);
// glVertex3d(vn.x(),vn.y(),z);
// }
// glEnd();
// }
}
void glDrawCairoSurface(const Cairo::RefPtr<Cairo::ImageSurface> surface,
const Vector2d &min, const Vector2d &max,
const double z)
{
if (surface==0) return;
int w = surface->get_width();
int h = surface->get_height();
unsigned char * data = surface->get_data();
GLuint texture; glGenTextures( 1, &texture );
glBindTexture( GL_TEXTURE_2D, texture );
// http://www.nullterminator.net/gltexture.html
// select modulate to mix texture with color for shading
glTexEnvf( GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_MODULATE );
// when texture area is small, bilinear filter the closest mipmap
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER,
GL_LINEAR_MIPMAP_NEAREST );
// when texture area is large, bilinear filter the original
glTexParameterf( GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR );
// build our texture mipmaps
gluBuild2DMipmaps( GL_TEXTURE_2D, GL_ALPHA, w, h,
GL_ALPHA, GL_UNSIGNED_BYTE, data );
glEnable(GL_TEXTURE_2D);
glBegin(GL_QUADS);
glTexCoord2d(0.0,0.0); glVertex3d(min.x(),min.y(),z);
glTexCoord2d(1.0,0.0); glVertex3d(max.x(),min.y(),z);
glTexCoord2d(1.0,1.0); glVertex3d(max.x(),max.y(),z);
glTexCoord2d(0.0,1.0); glVertex3d(min.x(),max.y(),z);
glEnd();
glDisable(GL_TEXTURE_2D);
glDeleteTextures( 1, &texture );
}
int clipSegmentLine(Vector2d& p1, Vector2d& p2, double a, double b, double c ){
bool p1inside = true;
bool p2inside = true;
if (a*p1.x()+b*p1.y()+c < 0){
p1inside=false;
}
if (a*p2.x()+b*p2.y()+c < 0){
p2inside=false;
}
if (p1inside && p2inside)
return 2;
if (!p1inside && !p2inside)
return -1;
Vector2d dp=p2-p1;
double den=a*dp.x()+b*dp.y();
if (den==0)
return -1;
double num = c + a*p1.x()+b*p1.y();
double t = - num/den;
if (p1inside){
p2=p1+dp*t;
return 1;
}
p1=p1+dp*t;
return 0;
}
// directed (one end is wider than the other)
vector<Poly> dir_thick_line(const Vector2d &from, const Vector2d &to,
double fr_width, double to_width)
{
vector<Poly> p;
if (fr_width < 0.001 || to_width < 0.001) return p;
if (to.squared_distance(from) < 0.001) return p;
if ((fr_width < 0) != (to_width < 0)) return p;
Poly poly;
Vector2d fdir = (to-from); fdir.normalize();
Vector2d tdir = fdir;
fdir *= fr_width/4.;
tdir *= to_width/4.;
Vector2d fr_dirp(-fdir.y(), fdir.x());
Vector2d to_dirp(-tdir.y(), tdir.x());
poly.addVertex(from-fdir-fr_dirp);
poly.addVertex(from-fdir+fr_dirp);
poly.addVertex(to+tdir+to_dirp);
poly.addVertex(to+tdir-to_dirp);
p.push_back(poly);
return p;
//return Clipping::getOffset(poly, distance/4, jmiter, 0);
// slow:
// poly.addVertex(from);
// poly.addVertex(to);
// return Clipping::getOffset(poly, distance/2, jround, distance/2.);
}
/*
* Iterate the vertices, and calculate whether this
* shape is a hole or enclosed, based on whether the
* segment normals point outward (a hole) or inward
* (enclosing)
*/
void Poly::calcHole() const // hole is mutable
{
if(vertices.size() < 3)
return; // hole is undefined
Vector2d p(-INFTY, -INFTY);
int v=0;
center = Vector2d(0,0);
Vector2d q;
for(size_t vert=0;vert<vertices.size();vert++)
{
q = vertices[vert];
center += q;
if(q.x() > p.x())
{
p = q;
v=vert;
}
else if(q.x() == p.x() && q.y() > p.y())
{
p.y() = q.y();
v=vert;
}
}
center /= vertices.size();
// we have the x-most vertex (with the highest y if there was a contest), v
Vector2d V1 = getVertexCircular(v-1);
Vector2d V2 = getVertexCircular(v);
Vector2d V3 = getVertexCircular(v+1);
// Vector2d Va=V2-V1;
// Vector2d Vb=V3-V2;
hole = isleftof(V2, V3, V1); //cross(Vb,Va) > 0;
holecalculated = true;
}
void center_perpendicular(const Vector2d &from, const Vector2d &to,
Vector2d &p1, Vector2d &p2)
{
Vector2d center = (from+to)/2.;
Vector2d dir = Vector2d(from.y()-to.y(), to.x()-from.x());
p1 = center;
p2 = center + dir;
}
void rotate(Vector2d &p, const Vector2d ¢er, double angle, bool ccw)
{
if (p==center) return ;
if (!ccw) angle = -angle;
double cosa = cos(angle);
double sina = sin(angle);
Vector2d r(p - center);
p.x() = center.x() + r.x() * cosa - r.y() * sina;
p.y() = center.y() + r.x() * sina + r.y() * cosa;
}
bool isFeetDirection(const Vector2d& v, int x, int y, const Vector2d& dir){
bool res = false;
int dx = x - v.x();
int dy = y - v.y();
if (dir.x())
res = abs(dy) <= kMinDist && dx * dir.x() >= 0;
else
res = abs(dx) <= kMinDist && dy * dir.y() >= 0;
return res;
}
int getCairoSurfaceDatapoint(const Cairo::RefPtr<Cairo::ImageSurface> surface,
const Vector2d &min, const Vector2d &max,
const Vector2d &p)
{
if (surface==0) return 0;
const int w = surface->get_stride();
const int h = surface->get_height();
const unsigned char * data = surface->get_data();
const Vector2d diag = max - min;
const Vector2d relp = p - min;
const int ipx = (int)(relp.x() / diag.x() * (double)w);
const int ipy = (int)(relp.y() / diag.y() * (double)h);
int value = data[ipy*w + ipx];
return value;
}
void MapOverview::ShowSamples(const QList<Robot::DetectedSample> &samples)
{
static const QFont font("Times", 3, QFont::Bold);
qDeleteAll(mSampleDetections->childItems());
for(auto& sample : samples)
{
Vector2d loc = sample.location;
auto circle = new QGraphicsEllipseItem(loc.x()-0.2, loc.y()-0.2, 0.4, 0.4, mSampleDetections);
//circle->setPos(circle->pos() + mRobotInstance->pos());
circle->setPen(QPen(Qt::red, 0));
//mSampleDetections->addToGroup(circle);
//std::cout << "Sample at " << loc.transpose() << "\n";
/*
auto text = new QGraphicsTextItem(sample.name);
text->setPos(loc.x(), loc.y());
text->setFont(font);
text->setDefaultTextColor(Qt::red);
mTagDetections->addToGroup(text);
*/
}
}
// The following functions
// (inSegment, intersect2D_Segments and dist3D_Segment_to_Segment)
// are licensed as:
//
// Copyright 2001, softSurfer (www.softsurfer.com)
// This code may be freely used and modified for any purpose
// providing that this copyright notice is included with it.
// SoftSurfer makes no warranty for this code, and cannot be held
// liable for any real or imagined damage resulting from its use.
// Users of this code must verify correctness for their application.
//
// inSegment(): determine if a point is inside a segment
// Input: a point P, and a collinear segment p1--p2
// Return: true = P is inside p1--p2
// fasle = P is not inside p1--p2
bool inSegment( const Vector2d &P, const Vector2d &p1, const Vector2d &p2)
{
if (p1.x() != p2.x()) { // S is not vertical
if (p1.x() <= P.x() && P.x() <= p2.x())
return true;
if (p1.x() >= P.x() && P.x() >= p2.x())
return true;
}
else { // S is vertical, so test y coordinate
if (p1.y() <= P.y() && P.y() <= p2.y())
return true;
if (p1.y() >= P.y() && P.y() >= p2.y())
return true;
}
return false;
}
bool Poly::vertexInside2(const Vector2d &point, double maxoffset) const
{
// Shoot a ray along +X and count the number of intersections.
// If n_intersections is even, return false, else return true
Vector2d EndP(point.x()+10000, point.y());
int intersectcount = 1; // we want to test if uneven
double maxoffsetSq = maxoffset*maxoffset;
Vector2d dummy;
for(size_t i=0; i<vertices.size();i++)
{
const Vector2d P1 = getVertexCircular(i-1);
const Vector2d P2 = vertices[i];
if (point_segment_distance_Sq(point, P1, P2, dummy) <= maxoffsetSq) return true;
// Skip horizontal lines, we can't intersect with them,
// because the test line is horizontal
if(P1.y() == P2.y())
continue;
Intersection hit;
if(IntersectXY(point,EndP,P1,P2,hit,maxoffset))
intersectcount++;
}
return (intersectcount%2==0);
}
bool Layer::setMinMax(const vector<Poly> &polys)
{
Vector2d NewMin = Vector2d( INFTY, INFTY);
Vector2d NewMax = Vector2d(-INFTY, -INFTY);
for (uint i = 0; i< polys.size(); i++) {
vector<Vector2d> minmax = polys[i].getMinMax();
NewMin.x() = min(NewMin.x(), minmax[0].x());
NewMin.y() = min(NewMin.y(), minmax[0].y());
NewMax.x() = max(NewMax.x(), minmax[1].x());
NewMax.y() = max(NewMax.y(), minmax[1].y());
}
if (NewMin==Min && NewMax==Max) return false;
Min = NewMin;
Max = NewMax;
return true;
}
//------------------------------------------------------------------------------
void Projectile::updateState()
{
Vector2d v = getVelocity();
if( isEqual( v.x(), 0.0 ) )
setState( sVertical );
else
setState( sHorizontal );
switch( getType() )
{
case ptBasic:
if( getRemainingLife() <= 0 )
{ setState( sExploding ); }
break;
case ptBullet:
{
if( hasIntersections() || getRemainingLife() <= 0)
{ setState( sExploding ); }
}
break;
case ptGrenade:
{
if( getRemainingLife() <= 0 )
{
setState( sExploding );
Physics& p = mpEngine->getPhysics();
p.explode( getPosition(), 4 * 32, getExplosionDamage(), *mpEngine );
}
}break;
default: break;
}
}
bool rasterpolys(const vector<Poly> &polys,
const Vector2d &min, const Vector2d &max, double resolution,
Cairo::RefPtr<Cairo::ImageSurface> &surface,
Cairo::RefPtr<Cairo::Context> &context)
{
Vector2d diag = max - min;
int width = (int)ceil(diag.x()/resolution);
int height = (int)ceil(diag.y()/resolution);
if (height <= 0 || width <= 0)
return false;
surface = Cairo::ImageSurface::create(Cairo::FORMAT_A8, width, height);
//surface->set_device_offset(-min.x()/resolution, -min.y()/resolution);
context = Cairo::Context::create (surface);
context->set_fill_rule(Cairo::FILL_RULE_WINDING);
context->set_antialias(Cairo::ANTIALIAS_DEFAULT);
context->scale(1/resolution, 1/resolution);
context->translate(-min.x(), -min.y());
context->set_source_rgb (0,0,0);
//cerr << min << endl <<getMatrix(context) << endl;
// draw boundary
// context->begin_new_path();
// context->set_line_width(0.5);
// context->move_to(min.x(),min.y());
// context->line_to(max.x(),min.y());
// context->line_to(max.x(),max.y());
// context->line_to(min.x(),max.y());
// // context->move_to(0,0);
// // context->line_to(diag.x(),0);
// // context->line_to(diag.x(),diag.y());
// // context->line_to(0,diag.y());
// context->close_path();
// context->stroke();
context->begin_new_path();
context->set_line_width(0);
for (uint i=0; i<polys.size(); i++) {
Vector2d v = polys[i][0];
context->move_to(v.x(),v.y());
for (uint j=0; j<polys[i].size(); j++) {
Vector2d v = polys[i][j];
context->line_to(v.x(),v.y());
}
}
context->fill();
return true;
}
vector<Vector2d> Poly::getMinMax() const{
double minx=6000,miny=6000;
double maxx=-6000,maxy=-6000;
vector<Vector2d> range;
range.resize(2);
Vector2d v;
for (uint i=0; i < vertices.size();i++){
v = vertices[i];
if (v.x()<minx) minx=v.x();
if (v.x()>maxx) maxx=v.x();
if (v.y()<miny) miny=v.y();
if (v.y()>maxy) maxy=v.y();
}
range[0] = Vector2d(minx,miny);
range[1] = Vector2d(maxx,maxy);
return range;
}
void getDistance(size_t x, size_t y, double &distance)
{
// std::cout << "Distance for " << x << " " << y << " CenterX " << xCenter << " " << yCenter << " " << ((int)(x - xCenter)) * scaleX << " "<< scaleX << " " << 4*0.1 << std::endl;
Vector2d posAligned = inverseOrientation * (Vector2d(x * scaleX, y * scaleY) - pos);
distance = boxLut->getDistanceToBox(posAligned.x(), posAligned.y());
}
bool EdgePointXY::write(std::ostream& os) const
{
Vector2d p = measurement();
os << p.x() << " " << p.y();
for (int i = 0; i < 2; ++i)
for (int j = i; j < 2; ++j)
os << " " << information()(i, j);
return os.good();
}
void GraphSLAM::checkCovariance(OptimizableGraph::VertexSet& vset){
///////////////////////////////////
// we need now to compute the marginal covariances of all other vertices w.r.t the newly inserted one
CovarianceEstimator ce(_graph);
ce.setVertices(vset);
ce.setGauge(_lastVertex);
ce.compute();
assert(!_lastVertex->fixed() && "last Vertex is fixed");
assert(_firstRobotPose->fixed() && "first Vertex is not fixed");
OptimizableGraph::VertexSet tmpvset = vset;
for (OptimizableGraph::VertexSet::iterator it = tmpvset.begin(); it != tmpvset.end(); it++){
VertexSE2 *vertex = (VertexSE2*) *it;
MatrixXd Pv = ce.getCovariance(vertex);
Matrix2d Pxy; Pxy << Pv(0,0), Pv(0,1), Pv(1,0), Pv(1,1);
SE2 delta = vertex->estimate().inverse() * _lastVertex->estimate();
Vector2d hxy (delta.translation().x(), delta.translation().y());
double perceptionRange =1;
if (hxy.x()-perceptionRange>0)
hxy.x() -= perceptionRange;
else if (hxy.x()+perceptionRange<0)
hxy.x() += perceptionRange;
else
hxy.x() = 0;
if (hxy.y()-perceptionRange>0)
hxy.y() -= perceptionRange;
else if (hxy.y()+perceptionRange<0)
hxy.y() += perceptionRange;
else
hxy.y() = 0;
double d2 = hxy.transpose() * Pxy.inverse() * hxy;
if (d2 > 5.99)
vset.erase(*it);
}
}
void Game::fragmentExplosion(Vector2d<float> pos) {
pos.setXY(pos.x()+ 298 - plyer->rect().x(),pos.y() + 208 - plyer->rect().y());
for (int i = 0; i < 30; i++) {
int ang = System::rnd(360);
double ttl = 300 + System::rnd(150);
double velocity = 1.4 + System::rnd(1000)/1000.0;
creaturesView()->particlesManager().addParticle(pos,ttl,ang,velocity,0.05, Game::darkGray);
}
}
void GridManager::AddBox(Vector2d p, Vector2d size, GridBox* box) {
if(box == nullptr) {
std::cout << "[GridManager::AddBox] Cannot add null box." << std::endl;
return;
}
CellConfig def_config;
def_config.pressure = GetPressure();
def_config.T = GetTemperature();
def_config.boundary_T = GetTemperature();
Vector2d& cell_size = Config::vCellSize;
box->p = Vector2i(p.x() / cell_size.x(), p.y() / cell_size.y());
box->size = Vector2i(size.x() / cell_size.x(), size.y() / cell_size.y());
box->def_config = def_config;
boxes_stack_.push_back(box);
}
//------------------------------------------------------------------------------
void Monster::updateAi()
{
assert(mpEngine);
//on les fait courrir après le joeur
Point2d playerPos = mpEngine->getPlayer().getPosition();
Point2d pos = getPosition();
Vector2d dir = playerPos - pos;
if( dir.x() < 0 ) moveLeft();
else moveRight();
if( dir.y() > 0 ) jump();
}
//------------------------------------------------------------------------------
//void GameEntity::setAcceleration( const Vector2d& iV )
//{ mAcceleration = iV; }
//------------------------------------------------------------------------------
void GameEntity::setBoundingBox( const Rectangle& iV )
{
mBoundingBox = iV;
mBoundingCircle.setRadius( sqrt( pow(iV.width()/2.0, 2) +
pow( iV.height() / 2.0, 2 ) ) );
const Stage& s = mpEngine->getStage();
const Vector2d bbSize = mBoundingBox.size();
int kernel = max( bbSize.x(), bbSize.y() ) /
max( s.getCellSize().x(), s.getCellSize().y() ) * 3;
kernel = max(kernel, 3); //le kernel doit au minimum etre de 3.
if( kernel % 2 == 0 ) ++kernel; //kernel ne doit pas etre pair
mCollisionSearchGrid.set( kernel, kernel );
}
bool fit_arc(const int m_dat, const arc_data_struct data,
const int n_par, double *par, double sq_error,
Vector2d &result_center, double &result_radiussq)
{
lm_status_struct status;
lm_control_struct control = lm_control_double;
control.printflags = 0; // 3 = monitor status (+1) and parameters (+2)
control.maxcall = 200;
control.ftol = sq_error; // max square error sum
// printf( "Fitting:\n" );
lmmin( n_par, par, m_dat, (const void*) &data,
evaluate_arcfit, &control, &status, lm_printout_std );
result_center.x() = par[0];
result_center.y() = par[1];
result_radiussq = par[2];
double fvec[m_dat];
int info;
evaluate_arcfit(par, m_dat, (const void*) &data, fvec, &info);
double totres = 0;
for (int i=0; i<m_dat; ++i ) totres+=fvec[i];
return (totres < sq_error);
#if 0
printf( "\nResults:\n" );
printf( "status after %d function evaluations:\n %s\n",
status.nfev, lm_infmsg[status.info] );
printf("obtained parameters:\n");
int i;
for ( i=0; i<n_par; ++i )
printf(" par[%i] = %12g\n", i, par[i]);
printf("obtained norm:\n %12g\n", status.fnorm );
#endif
// printf("fitting data as follows:\n");
// double ff;
// for ( i=0; i<m_dat; ++i ){
// ff = f(tx[i], tz[i], par);
// printf( " t[%2d]=%12g,%12g y=%12g fit=%12g residue=%12g\n",
// i, tx[i], tz[i], y[i], ff, y[i] - ff );
// }
// free(data.px);
// free(data.py);
}
int QuadtreeNode<Item>::Quadrant(const Vector2d &pos) const {
if (!rect_.IsPositionIn(pos)) {
return -1;
}
if (pos.x() < center_.x()) {
if (pos.y() < center_.y()) {
return 0; // Top Left
} else {
return 2; // Bottom Left
}
} else {
if (pos.y() < center_.y()) {
return 1; // Top Right
} else {
return 3; // Bottom Right
}
}
}
vector<Poly> thick_line(const Vector2d &from, const Vector2d &to, double width)
{
vector<Poly> p;
if (width < 0.001) return p;
if (to.squared_distance(from) < 0.001) return p;
Poly poly;
Vector2d dir = (to-from); dir.normalize(); dir *= width/4.;
Vector2d dirp(-dir.y(),dir.x());
poly.addVertex(from-dir-dirp);
poly.addVertex(from-dir+dirp);
poly.addVertex(to+dir+dirp);
poly.addVertex(to+dir-dirp);
p.push_back(poly);
return Clipping::getOffset(poly, width/4, jmiter, 0);
// slow:
// poly.addVertex(from);
// poly.addVertex(to);
// return Clipping::getOffset(poly, distance/2, jround, distance/2.);
}
// find center for best fit of arcpoints
bool fit_arc(const vector<Vector2d> &points, double sq_error,
Vector2d &result_center, double &result_radiussq)
{
if (points.size() < 3) return false;
const int n_par = 3; // center x,y and arc radius_sq
// start values:
const Vector2d P = points[0];
const Vector2d Q = points.back();
const Vector2d startxy = (P+Q)/2.;
double par[3] = { startxy.x(), startxy.y(), P.squared_distance(Q) };
int m_dat = points.size();
arc_data_struct data;
data.px = new double[m_dat];
data.py = new double[m_dat];
for (int i = 0; i < m_dat; i++) {
data.px[i] = points[i].x();
data.py[i] = points[i].y();
}
return fit_arc(m_dat, data, n_par, par, sq_error,
result_center, result_radiussq);
}
// http://paulbourke.net/geometry/insidepoly/
bool Poly::vertexInside(const Vector2d &p, double maxoffset) const
{
#define POLYINSIDEVERSION 0
#if POLYINSIDEVERSION==0
// this one works
uint N = size();
if (N < 2) return false;
uint counter = 0;
uint i;
double xinters;
const Vector2d *p1, *p2;
p1 = &(vertices[0]);
for (i=1;i<=N;i++) {
p2 = &(vertices[i % N]);
if (p.y() > min(p1->y(), p2->y())) {
if (p.y() <= max(p1->y(), p2->y())) {
if (p.x() <= max(p1->x(), p2->x())) {
if (p1->y() != p2->y()) {
xinters = (p.y()-p1->y())*(p2->x()-p1->x())/(p2->y()-p1->y())+p1->x();
if (p1->x() == p2->x() || p.x() <= xinters)
counter++;
}
}
}
}
p1 = p2;
}
return (counter % 2 != 0);
#else
// not really working?
bool c = false;
//Poly off = Clipping::getOffset(*this,maxoffset).front();
for (uint i = 0; i < vertices.size(); i++) {
const Vector2d Pi = vertices[i];
const Vector2d Pj = getVertexCircular(i+1);
if ( ((Pi.y() > p.y()) != (Pj.y() > p.y())) &&
(abs(p.x() - (Pj.x()-Pi.x())
* (p.y()-Pi.y()) / (Pj.y()-Pi.y()) + Pi.x()) > maxoffset) )
c = !c;
}
if (!c)
for (uint i = 0; i < vertices.size(); i++)
if ((vertices[i]-p).length() < maxoffset) return true; // on a vertex
return c;
#endif
}
vector<Segment> Shape::getCutlines(const Matrix4d &T, double z,
vector<Vector2d> &vertices,
double &max_gradient,
vector<Triangle> &support_triangles,
double supportangle,
double thickness) const
{
Vector2d lineStart;
Vector2d lineEnd;
vector<Segment> lines;
// we know our own tranform:
Matrix4d transform = T * transform3D.transform ;
int count = (int)triangles.size();
// #ifdef _OPENMP
// #pragma omp parallel for schedule(dynamic)
// #endif
for (int i = 0; i < count; i++)
{
Segment line(-1,-1);
int num_cutpoints = triangles[i].CutWithPlane(z, transform, lineStart, lineEnd);
if (num_cutpoints == 0) {
if (supportangle >= 0 && thickness > 0) {
if (triangles[i].isInZrange(z-thickness, z, transform)) {
const double slope = -triangles[i].slopeAngle(transform);
if (slope >= supportangle) {
support_triangles.push_back(triangles[i].transformed(transform));
}
}
}
continue;
}
if (num_cutpoints > 0) {
int havev = find_vertex(vertices, lineStart);
if (havev >= 0)
line.start = havev;
else {
line.start = vertices.size();
vertices.push_back(lineStart);
}
if (abs(triangles[i].Normal.z()) > max_gradient)
max_gradient = abs(triangles[i].Normal.z());
if (supportangle >= 0) {
const double slope = -triangles[i].slopeAngle(transform);
if (slope >= supportangle)
support_triangles.push_back(triangles[i].transformed(transform));
}
}
if (num_cutpoints > 1) {
int havev = find_vertex(vertices, lineEnd);
if (havev >= 0)
line.end = havev;
else {
line.end = vertices.size();
vertices.push_back(lineEnd);
}
}
// Check segment normal against triangle normal. Flip segment, as needed.
if (line.start != -1 && line.end != -1 && line.end != line.start)
{ // if we found a intersecting triangle
Vector3d Norm = triangles[i].transformed(transform).Normal;
Vector2d triangleNormal = Vector2d(Norm.x(), Norm.y());
Vector2d segment = (lineEnd - lineStart);
Vector2d segmentNormal(-segment.y(),segment.x());
triangleNormal.normalize();
segmentNormal.normalize();
if( (triangleNormal-segmentNormal).squared_length() > 0.2){
// if normals do not align, flip the segment
int iswap=line.start;line.start=line.end;line.end=iswap;
}
// cerr << "line "<<line.start << "-"<<line.end << endl;
lines.push_back(line);
}
}
return lines;
}
/**
* @brief idle
*/
void idle()
{
double deltaT = (double)TIMER_MS;
optotrak.updateMarkers(deltaT);
markers = optotrak.getAllMarkers();
// Coordinates picker
allVisiblePlatform = isVisible(markers.at(15).p) && isVisible(markers.at(16).p);
allVisibleThumb = isVisible(markers.at(11).p) && isVisible(markers.at(12).p) && isVisible(markers.at(13).p);
allVisibleIndex = isVisible(markers.at(7).p) && isVisible(markers.at(8).p) && isVisible(markers.at(9).p);
allVisibleFingers = allVisibleThumb && allVisibleIndex;
allVisiblePatch = isVisible(markers.at(1).p) && isVisible(markers.at(2).p) && isVisible(markers.at(3).p);
allVisibleHead = allVisiblePatch && isVisible(markers.at(17).p) && isVisible(markers.at(18).p);
//if ( allVisiblePatch )
headEyeCoords.update(markers.at(1).p,markers.at(2).p,markers.at(3).p,deltaT);
// update thumb coordinates
thumbCoords.update(markers.at(11).p,markers.at(12).p,markers.at(13).p,deltaT);
// update index coordinates
indexCoords.update(markers.at(7).p, markers.at(8).p, markers.at(9).p,deltaT);
// Compute the coordinates of visual thumb
thumb = thumbCoords.getP1().p;
index = indexCoords.getP1().p;
eyeLeft = headEyeCoords.getLeftEye().p;
eyeRight = headEyeCoords.getRightEye().p;
rigidCurrentThumb.setRigidBody(markers.at(11).p,markers.at(12).p,markers.at(13).p);
rigidCurrentIndex.setRigidBody(markers.at(7).p,markers.at(8).p,markers.at(9).p);
rigidStartThumb.computeTransformation(rigidCurrentThumb);
rigidStartIndex.computeTransformation(rigidCurrentIndex);
if (headCalibrationDone==3 && fingerCalibrationDone==4)
{
if ( !allVisibleIndex || !allVisibleThumb || !isVisible(markers.at(6).p))
boost::thread invisibleBeep( beepInvisible);
Vector2d thumbProjection = cam.computeProjected(thumb);
thumbProjectedInside = ( thumbProjection.x() >= 0 && thumbProjection.x() <= SCREEN_WIDTH ) && ( thumbProjection.y() >= 0 && thumbProjection.y() <= SCREEN_HEIGHT );
Vector2d indexProjection = cam.computeProjected(index);
indexProjectedInside = ( indexProjection.x() >= 0 && indexProjection.x() <= SCREEN_WIDTH ) && ( indexProjection.y() >= 0 && indexProjection.y() <= SCREEN_HEIGHT );
trialFrame++;
// Work on fingers coordinates
double wThumb=0.5, wIndex=0.5;
Vector3d m = (wThumb*thumb+wIndex*index)/(wThumb+wIndex);
Vector3d d = thumb-index;
visualThumb = thumb;
visualIndex = index;
double clamp= 2*factors.getCurrent().at("StimulusRadius");
double gain = 1.0;//in quello di Bob era invece: factors.getCurrent().at("Gain");
if ( d.norm() > clamp )
{
visualThumb = m + 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1));
visualIndex = m - 0.5*d.normalized()*(gain*d.blueNorm() - clamp*(gain-1));
}
// Check when to advance trial
double stimRadius = factors.getCurrent().at("StimulusRadius");
double radiusTolerance = util::str2num<double>(parameters.find("RadiusTolerance"));
fingNow = fingersInSphericalVolume(thumb,index, visualStimCenter, stimRadius-radiusTolerance, stimRadius+radiusTolerance, selectedFinger );
// SubjectName\tFingerDist\tTrialNumber\tTrialFrame\tTotTime\tVisualStimX\tVisualStimY\tVisualStimZ\tfStimulusRadius\tfDistances\tfGain\tEyeLeftXraw\tEyeLeftYraw\tEyeLeftZraw\tEyeRightXraw\tEyeRightYraw\tEyeRightZraw\tWristXraw\tWristYraw\tWristZraw\tThumbXraw\tThumbYraw\tThumbZraw\tIndexXraw\tIndexYraw\tIndexZraw\tVisualThumbXraw\tVisualThumbYraw\tVisualThumbZraw\tVisualIndexXraw\tVisualIndexYraw\tVisualIndexZraw\tIsDrawing\tIsThumbProjected\tIsIndexProjected
if (!isSaving)
return;
RowVector3d junk(9999,9999,9999);
markersFile << fixed << setprecision(3) <<
parameters.find("SubjectName") << "\t" <<
fingerDistance << "\t" <<
totalTrialNumber << "\t" <<
trialFrame << "\t" <<
globalTimer.getElapsedTimeInMilliSec() << "\t" <<
visualStimCenter.x() << "\t" <<
visualStimCenter.y() << "\t" <<
visualStimCenter.z() << "\t" <<
factors.getCurrent().at("StimulusRadius") << "\t" <<
factors.getCurrent().at("Distances") << "\t" <<
factors.getCurrent().at("Gain") << "\t" <<
factors.getCurrent().at("DisappearRadius") << "\t" <<
( isVisible(eyeLeft) ? eyeLeft.transpose() : junk ) << "\t" <<
( isVisible(eyeRight) ? eyeRight.transpose() : junk ) << "\t" <<
( isVisible(markers.at(6).p) ? markers.at(6).p.transpose() : junk ) << "\t" <<
( isVisible(thumb) ? thumb.transpose() : junk ) << "\t" <<
( isVisible(index) ? index.transpose() : junk ) << "\t" <<
( isVisible(visualThumb) ? visualThumb.transpose() : junk ) << "\t" <<
( isVisible(visualIndex) ? visualIndex.transpose() : junk ) << "\t" <<
isDrawing << "\t" <<
thumbProjectedInside << "\t" <<
indexProjectedInside << "\t" <<
fingNow <<
endl;
// Switch to this function if you want cylindrical stimulus
//fingersInCylindricalVolume( thumb,index, Vector3d(0,0,factors.getCurrent().at("Distances")), util::str2num<double>(parameters.find("StimulusHeight")), stimRadius,stimRadius+20, selectedFinger );
if (trialMode == STIMULUSMODE )
{
if ( !fingNow ) // always starts counting the time if the finger is outside
{
fingersTimer.start();
}
// only if the finger is inside the volume the counter has counted more that 1 second
if ( fingersTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("LeaningTime")) )
{
plato_write(PLATO_LEFT_RIGHT_CLOSED);
advanceTrial();
plato_write(PLATO_LEFT_RIGHT_OPEN);
}
trialTimer.start(); // continue keeping it starting during stimulus
drawingTrialFrame++;
invisibleIndexFrames+=!allVisibleIndex;
invisibleThumbFrames+=!allVisibleThumb;
invisibleWristFrames+=!isVisible(markers.at(6).p);
}
else
{
if ( trialTimer.getElapsedTimeInMilliSec() > util::str2num<double>(parameters.find("IntervalTime")) )
{
boost::thread trialBeep( beepTrial );
trialMode=STIMULUSMODE;
}
}
isDrawing = (trialMode == STIMULUSMODE);
}
}
