void TabPreview::showOnRect(const QRect &r)
{
if (m_animation.state() == QTimeLine::Running) {
m_animation.stop();
}
m_startGeometry = geometry();
bool wasVisible = isVisible();
QRect finishingGeometry;
resize(QSize(250, 170));
QFrame::show();
if (m_pixmapLabel->isVisible()) {
m_title->setWordWrap(false);
m_title->setText(m_title->fontMetrics().elidedText(m_title->text(), Qt::ElideRight, 240));
QSize previewSize(250, 170);
finishingGeometry = QRect(calculatePosition(r, previewSize), previewSize);
}
else {
m_title->setWordWrap(true);
QSize previewSize = sizeHint();
previewSize.setWidth(qMin(previewSize.width() + 2 * 5, 240));
previewSize.setHeight(qMin(previewSize.height() + 2 * 5, 130));
finishingGeometry = QRect(calculatePosition(r, previewSize), previewSize);
}
#ifdef ENABLE_OPACITY_EFFECT
if (!m_animationsEnabled) {
m_opacityEffect.setOpacity(1.0);
#else
if (!m_animationsEnabled || !wasVisible) {
#endif
QFrame::setGeometry(finishingGeometry);
return;
}
else {
showAnimated();
}
if (!wasVisible) {
m_startGeometry = finishingGeometry;
}
QFrame::setGeometry(m_startGeometry);
calculateSteps(m_startGeometry, finishingGeometry);
m_animation.start();
}
#ifdef ENABLE_OPACITY_EFFECT
void TabPreview::setOpacity(int opacity)
{
m_opacityEffect.setOpacity(opacity / 100.0);
}
/**
* Handler for the reset event.
*/
void PeaksViewerOverlayDialog::onReset() {
m_peaksPresenter->setPeakSizeOnProjection(m_originalOnProjectionFraction);
m_peaksPresenter->setPeakSizeIntoProjection(m_originalIntoProjectionFraction);
ui->sliderOnProjection->setSliderPosition(
calculatePosition(m_originalOnProjectionFraction));
ui->sliderIntoProjection->setSliderPosition(
calculatePosition(m_originalIntoProjectionFraction));
ui->lblPercentageOnProjection->setText(
formattedPercentageValue(m_originalOnProjectionFraction));
ui->lblPercentageIntoProjection->setText(
formattedPercentageValue(m_originalIntoProjectionFraction));
}
void TabPreview::showOnRect(const QRect &r)
{
if (m_animation->state() == QPropertyAnimation::Running) {
m_animation->stop();
}
QRect oldGeometry = geometry();
bool wasVisible = isVisible();
resize(QSize(250, 170));
QFrame::show();
QRect finishingGeometry;
if (m_pixmap->isVisible()) {
m_title->setWordWrap(false);
m_title->setText(m_title->fontMetrics().elidedText(m_title->text(), Qt::ElideRight, 240));
QSize previewSize(250, 170);
finishingGeometry = QRect(calculatePosition(r, previewSize), previewSize);
}
else {
m_title->setWordWrap(true);
QSize previewSize = sizeHint();
previewSize.setWidth(qMin(previewSize.width() + 2 * 5, 240));
previewSize.setHeight(qMin(previewSize.height() + 2 * 5, 130));
finishingGeometry = QRect(calculatePosition(r, previewSize), previewSize);
}
if (!m_animationsEnabled) {
m_opacityEffect->setOpacity(1.0);
QFrame::setGeometry(finishingGeometry);
return;
}
else {
showAnimated();
}
if (!wasVisible) {
oldGeometry = finishingGeometry;
}
setFinishingGeometry(oldGeometry, finishingGeometry);
m_animation->start();
}
void toweringinferno::heatvision::HeatvisionSystem::update(
const toweringinferno::World& world
)
{
for(auto civilianIt = m_civilians.begin(); civilianIt != m_civilians.end(); ++civilianIt)
{
assert(civilianIt->isAlive());
const Cell& cell = world.getCell(civilianIt->pos);
const float hpDelta
= cell.fire > 0.0f ? utils::mapValue(cell.fire, 0.0f, 0.5f, 0.0f, -0.4f)
: cell.water > 0.0f ? utils::mapValue(cell.water, 0.85f, 1.5f, 0.0f, -0.4f)
: 0.0f;
civilianIt->hp = utils::max(0.0f, civilianIt->hp + hpDelta);
TileHeat heat[eTile_Count];
const Point& origin = civilianIt->pos;
for(int tileIndex = 0; tileIndex < eTile_Count; ++tileIndex)
{
const Tile currentTile = static_cast<Tile>(tileIndex);
const Point tilePos = calculatePosition(origin, currentTile);
const float danger = calculateBleedableDanger(tilePos, world);
const float desire = calculateDesire(tilePos, origin);
heat[tileIndex] = TileHeat(tilePos, danger, desire);
}
gaussianBlur(heat, 0.2f, civilianIt->heatMap);
for(int tileIndex = 0; tileIndex < eTile_Count; ++tileIndex)
{
const Tile currentTile = static_cast<Tile>(tileIndex);
const Point tilePos = calculatePosition(origin, currentTile);
const float danger = calculateNonBleedableDanger(tilePos, currentTile, world, m_civilians.begin(), m_civilians.end());
civilianIt->heatMap[tileIndex].danger = utils::max(danger, civilianIt->heatMap[tileIndex].danger);
}
std::sort(civilianIt->heatMap, civilianIt->heatMap + eTile_Count);
civilianIt->nextPos = civilianIt->heatMap[0].pos;
}
}
TEST_F(ContactConstraintGenerationTests, LocalPoses)
{
//Move the collision representation away from the physics representation for the sphere
collision0->setLocalPose(makeRigidTransform(Quaterniond::Identity(), Vector3d(5.0, 0.0, 0.0)));
std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(collision0, collision1);
SurgSim::Collision::SphereDoubleSidedPlaneDcdContact contactCalculation;
contactCalculation.calculateContact(pair);
ASSERT_EQ(1u, pair->getContacts().size());
pairs.push_back(pair);
state->setCollisionPairs(pairs);
ContactConstraintGeneration generator;
generator.update(0.1, state);
ASSERT_EQ(1u, state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT).size());
auto constraint = state->getConstraintGroup(CONSTRAINT_GROUP_TYPE_CONTACT)[0];
auto localization = constraint->getLocalizations().first;
auto location = pair->getContacts().front()->penetrationPoints.first;
Vector3d localizationGlobalPosition = localization->calculatePosition();
Vector3d locationGlobalPosition = collision0->getPose() * location.rigidLocalPosition.getValue();
EXPECT_TRUE(localizationGlobalPosition.isApprox(locationGlobalPosition)) <<
"The contact location is not in the same position as the localization produced by constraint generation";
}
void AKOpenNIUserFrameGenerator::setJointProperties(AKOpenNISkeletonJoint& openNISkeletonJoint, XnUserID user, XnSkeletonJoint eJoint)
{
XnSkeletonJointPosition jointPosition;
_userGenerator->GetSkeletonCap().GetSkeletonJointPosition(user, eJoint, jointPosition);
calculatePosition(openNISkeletonJoint.skeletonJoint->position, jointPosition.position);
openNISkeletonJoint.skeletonJoint->positionConfidence = jointPosition.fConfidence;
XnSkeletonJointOrientation orientation;
_userGenerator->GetSkeletonCap().GetSkeletonJointOrientation(user, eJoint, orientation);
openNISkeletonJoint.orientationConfidence = orientation.fConfidence;
openNISkeletonJoint.orientation.identity();
if(openNISkeletonJoint.orientationConfidence > 0.0)
{
openNISkeletonJoint.orientation.M11 = orientation.orientation.elements[0];
openNISkeletonJoint.orientation.M12 = orientation.orientation.elements[3];
openNISkeletonJoint.orientation.M13 = orientation.orientation.elements[6];
openNISkeletonJoint.orientation.M21 = orientation.orientation.elements[1];
openNISkeletonJoint.orientation.M22 = orientation.orientation.elements[4];
openNISkeletonJoint.orientation.M23 = orientation.orientation.elements[7];
openNISkeletonJoint.orientation.M31 = orientation.orientation.elements[2];
openNISkeletonJoint.orientation.M32 = orientation.orientation.elements[5];
openNISkeletonJoint.orientation.M33 = orientation.orientation.elements[8];
}
}
void prVictim::setup(string _name, float _signal)
{
name = _name;
signal = _signal;
fontSize = 32;
animColor.setColor( ofColor::black );
animColor.setDuration( 0.3f );
animColor.setRepeatType(PLAY_ONCE);
animColor.setCurve(SWIFT_GOOGLE);
hue = hueFromSignalStrength(signal);
ofColor color;
color.setHsb(hue, 255, 255);
animColor.animateTo( color );
fontOpacity.reset( 255 );
fontOpacity.setRepeatType( PLAY_ONCE );
fontOpacity.setCurve( LATE_EASE_IN_EASE_OUT );
fontOpacity.setDuration(5);
fontOpacity.animateTo(0);
unica.loadFont("NeueHaasUnicaPro-Regular.ttf", fontSize);
minSig = 0;
maxSig = 100;
minHue = 75;
maxHue = 255;
// only random part in whole installation!
// TODO: find a parpameter so this does not have to be random!
angle = ofRandom(0, 360);
calculatePosition();
}
void ScrollViewBar::onScrolled(const Vec2& outOfBoundary)
{
if(_autoHideEnabled)
{
_autoHideRemainingTime = _autoHideTime;
ProtectedNode::setOpacity(_opacity);
}
Layout* innerContainer = _parent->getInnerContainer();
float innerContainerMeasure = 0;
float scrollViewMeasure = 0;
float outOfBoundaryValue = 0;
float innerContainerPosition = 0;
if(_direction == ScrollView::Direction::VERTICAL)
{
innerContainerMeasure = innerContainer->getContentSize().height;
scrollViewMeasure = _parent->getContentSize().height;
outOfBoundaryValue = outOfBoundary.y;
innerContainerPosition = -innerContainer->getPositionY();
}
else if(_direction == ScrollView::Direction::HORIZONTAL)
{
innerContainerMeasure = innerContainer->getContentSize().width;
scrollViewMeasure = _parent->getContentSize().width;
outOfBoundaryValue = outOfBoundary.x;
innerContainerPosition = -innerContainer->getPositionX();
}
float length = calculateLength(innerContainerMeasure, scrollViewMeasure, outOfBoundaryValue);
Vec2 position = calculatePosition(innerContainerMeasure, scrollViewMeasure, innerContainerPosition, outOfBoundaryValue, length);
updateLength(length);
setPosition(position);
}
void prVictim::update(float _minSig, float _maxSig, float _minHue, float _maxHue)
{
minSig = _minSig;
maxSig = _maxSig;
minHue = _minHue;
maxHue = _maxHue;
angle = angle >= 360 ? 0 + 0.02 : angle + 0.02;
calculatePosition();
fontOpacity.update( 1.0f/60.0f );
animColor.update( 1.0f/60.0f );
if (fontOpacity.hasFinishedAnimating()) {
finished = true;
}
for (int i=0; i < probeRequests.size(); i++) {
if (probeRequests.at(i).finished) {
probeRequests.erase(probeRequests.begin() + i);
} else {
ofColor color = animColor.getCurrentColor();
probeRequests.at(i).update(x, y, i, color);
}
}
}
void prSingleProbeRequest::setup(string _name, float _signal, float _minSig, float _maxSig, float _minHue, float _maxHue)
{
name = _name;
finished = false;
signal = _signal;
minSig = _minSig;
maxSig = _maxSig;
minHue = _minHue;
maxHue = _maxHue;
fontOpacity.reset( 255 );
fontOpacity.setRepeatType( PLAY_ONCE );
fontOpacity.setCurve( LATE_EASE_IN_EASE_OUT );
fontOpacity.setDuration(5);
fontOpacity.animateTo(0);
hue = hueFromSignalStrength(signal);
color.setHsb(hue, 255, 255);
int fontSize = 32;
if (signal > 80){
fontSize = 10; // 300
} else if (signal < 80 && signal > 40) {
fontSize = 28; // 150
} else if (signal < 40 && signal > 20) {
fontSize = 37; // 25
} else if (signal < 20 && signal > 0) {
fontSize = 42;
}
unica.loadFont("NeueHaasUnicaPro-Regular.ttf", fontSize);
angle = ofRandom(0, 360);
calculatePosition();
}
/*
* Test +x/+y quadrant
*/
TEST(AssignPositions, NegXNegYFindFifth) {
Project2Sample::R_ID leader;
leader.R_ID = 0;
leader.x = -10.0;
leader.y = -10.0;
vector<float> robotGroupInfo = calculatePosition(leader, 5);
EXPECT_FLOAT_EQ(-16.187184, robotGroupInfo.at(0)); //newX
EXPECT_FLOAT_EQ(-16.187184, robotGroupInfo.at(1)); //newY
EXPECT_FLOAT_EQ(45, robotGroupInfo.at(2)); //leaderTheta
}
/*
* Test +x/+y quadrant of leader
*/
TEST(AssignPositions, RotateLeader) {
Project2Sample::R_ID leader;
leader.R_ID = 0;
leader.x = 10.0;
leader.y = 10.0;
vector<float> robotGroupInfo = calculatePosition(leader, 0);
EXPECT_EQ(10, robotGroupInfo.at(0)); //newX
EXPECT_EQ(10, robotGroupInfo.at(1)); //newY
EXPECT_EQ(225, robotGroupInfo.at(2)); //leaderTheta
}
void Animutator::update(){
GLfloat time = glfwGetTime() - _startTime;
int n = floor(time / _duration);
time = time - (_duration * n);
int index = getIndexForTime(time);
GLfloat percent = getPercentOfKeyFrame(index, time);
_position = calculatePosition(index, percent);
_rotation_magnitude = calculateRotationMagnitude(index, percent);
_rotation_axis = calculateRotationAxis(index, percent);
_scale = calculateScale(index, percent);
}
void prSingleProbeRequest::update(float _minSig, float _maxSig, float _minHue, float _maxHue)
{
minSig = _minSig;
maxSig = _maxSig;
minHue = _minHue;
maxHue = _maxHue;
angle = angle >= 360 ? 0 + 0.02: angle + 0.02;
calculatePosition();
fontOpacity.update( 1.0f/60.0f );
if (fontOpacity.hasFinishedAnimating()) {
finished = true;
}
}
void DriverRadarPositioner::setStartupPosition()
{
int laps = EventData::getInstance().getEventInfo().laps;
lapped = false;
qualiOut = false;
inPits = false;
finished = false;
wetTrack = false;
sectorPositions[0] = 0;
sectorPositions[1] = 0;
//a very rough estimate ;)
avgTime = 200 - 30 * log10(laps*laps);
if (EventData::getInstance().getSessionRecords().getFastestLap().getTime().isValid())
avgTime = EventData::getInstance().getSessionRecords().getFastestLap().getTime().toDouble();
//if it's raining, we add 10 seconds
if (EventData::getInstance().getWeather().getWetDry().getValue() == 1)
avgTime += 10;
//if SC is on track, we add 1 minute
if (EventData::getInstance().getFlagStatus() == LTPackets::SAFETY_CAR_DEPLOYED)
avgTime += 60;
if (driverData && (driverData->isInPits() || driverData->isRetired()))
{
inPits = true;
calculatePitPosition();
}
else if (EventData::getInstance().getEventType() == LTPackets::RACE_EVENT &&
(!EventData::getInstance().isSessionStarted() || !driverData->getLastLap().getSectorTime(1).isValid()))
{
inPits = false;
currentDeg = 360 - driverData->getPosition()*2;
currentLapTime = -(avgTime-(avgTime * currentDeg) / 360);
}
else
{
currentLapTime = 0;
for (int i = 0; i < 3; ++i)
currentLapTime += driverData->getLastLap().getSectorTime(i+1).toDouble();
calculatePosition();
}
}
FoTableCellView::FoTableCellView(FoTableRowView *pRowParent,ObjectFoContainerDoc *doc,MainView *pMainView)
:ObjectFoContainerSimpleView(pRowParent,doc,pMainView),m_pTableRowView(pRowParent)
{
//calculate pos according to parent
QGraphicsRectItem::setPos(calculatePosition());
//calculate width according to parent
int width=foTableCellDoc()->widthValue();
int height=foTableCellDoc()->height();
setRect(0,0,width,height);
setZValue ( PageView::ESimpleFoObjectViewZPosition );
//set item selectable
setFlag(QGraphicsItem::ItemIsSelectable, true);
}
void DriverRadarPositioner::update()
{
if (startingNewLap && driverData && driverData->getLastLap().getSectorTime(3).isValid())
{
currSector = 1;
currentLapTime = 0;
currentDeg = 0;
calculateAvgs();
startingNewLap = false;
}
// else
{
if (EventData::getInstance().getEventType() == LTPackets::RACE_EVENT && driverData->getLastLap().getSectorTime(3).isValid() &&
EventData::getInstance().getCompletedLaps() == EventData::getInstance().getEventInfo().laps && (finished || fabs(maxDeg() - currentDeg) < 5))
{
currentDeg = 0;
finished = true;
inPits = true;
calculatePitPosition();
}
else if (driverData->isInPits() || driverData->isRetired() ||
(EventData::getInstance().getEventType() == LTPackets::RACE_EVENT && EventData::getInstance().getFlagStatus() == LTPackets::RED_FLAG))
{
inPits = true;
finished = false;
calculatePitPosition();
}
else
{
if (inPits)
calculatePitOutPosition();
inPits = false;
finished = false;
calculatePosition();
}
}
if (!driverData->getLastLap().getSectorTime(3).isValid())
{
startingNewLap = true;
}
}
void LinkedStructure::moveBy(float dx, float dy)
{
// Here we move by dx an dy the linked structure's tip
VectorXf dAngles = VectorXf::Zero(mList.size(), 1);
VectorXf dPosition = VectorXf::Zero(2, 1);
dPosition(0) = dx;
dPosition(1) = dy;
dAngles = pseudoInverse() * dPosition;
// Adding to the angles the different required
// to move to the new position
for (int i = 0; i < mList.size(); i++)
mList[i]->mAngle += dAngles(i);
calculatePosition();
}
void AKOpenNIUserFrameGenerator::generateUserFrame()
{
XnUserID *aUsers = new XnUserID[_maxSkeletons];
XnUInt16 nUsers = _maxSkeletons;
XnUInt16 trackedUsers = _userGenerator->GetNumberOfUsers();
XnPoint3D position;
XnStatus rc;
_userGenerator->GetUsers(aUsers, nUsers);
_userFrame->frameNumber = _userGenerator->GetFrameID();
_userFrame->timeStamp = (int) (_userGenerator->GetTimestamp() / 1000);
for (int i = 0; i < _maxSkeletons; ++i)
{
if(i < trackedUsers)
{
rc = _userGenerator->GetCoM(aUsers[i], position);
_userFrame->users[i].isTracking = true;
_userFrame->users[i].userID = aUsers[i];
_userFrame->users[i].trackingID = aUsers[i];
_userFrame->users[i].hasSkeleton = (_skeletonTrackingEnabled && _userGenerator->GetSkeletonCap().IsTracking(aUsers[i]));
calculatePosition(_userFrame->users[i].position, position);
if (_userFrame->users[i].hasSkeleton)
{
addJointElements(_openNIUserFrame->openNIUsers[i], aUsers[i]);
addBoneElements(_openNIUserFrame->openNIUsers[i], aUsers[i]);
}
}
else
{
_userFrame->users[i].isTracking = false;
}
}
}
void XAP_Draw_Symbol::drawarea(UT_UCSChar c, UT_UCSChar p)
//
// This function displays the symbol c into the Selected Area.
// It also highlights the selected symbol in the Symbol Table.
//
{
UT_ASSERT(m_areagc);
UT_uint32 wwidth,wheight,x,y,cx,cy,px,py /*,swidth,sheight*/;
UT_uint32 /*cx1,cy1,*/px1,py1;
GR_Painter areaPainter(m_areagc);
GR_Painter painter(m_gc);
wwidth = m_drawareaWidth;
wheight = m_drawareaHeight;
// Center the character
// Note: That's bogus. measureString will give us the horizontal advance of "c",
// but we need the bounding box of "c" instead. To get it, we should use with FreeType face->bbox,
// in windows we should use the (FIXME: find the right name, it was something as getCharABC(...)
UT_uint32 h1 = 0;
UT_sint32 w1 = m_areagc->measureUnRemappedChar(c, &h1);
areaPainter.clearArea(0, 0, wwidth, wheight);
if(w1 != GR_CW_ABSENT)
{
x = (m_drawareaWidth - w1) / 2;
y = (m_drawareaHeight - h1) / 2;
areaPainter.drawChars(&c, 0, 1, x, y);
}
//
// Calculate the cordinates of the current and previous symbol
// along with the widths of the appropriate boxes.
// swidth = m_drawWidth;
// sheight = m_drawHeight;
UT_uint32 tmpw = m_drawWidth / 32;
UT_uint32 tmph = m_drawHeight / 7;
calculatePosition(c, cx, cy);
UT_sint32 wc = m_gc->measureUnRemappedChar(c);
cx *= tmpw;
cy *= tmph;
// cx1 = cx + tmpw;
// cy1 = cy + tmph;
calculatePosition(p, px, py);
UT_sint32 wp = m_gc->measureUnRemappedChar(p);
px *= tmpw;
py *= tmph;
px1 = px + tmpw;
py1 = py + tmph;
// Redraw the Previous Character in black on White
painter.clearArea(px + m_areagc->tlu(1), py + m_areagc->tlu(1), tmpw - m_areagc->tlu(1), tmph - m_areagc->tlu(1));
if(wp != GR_CW_ABSENT)
{
painter.drawChars(&p, 0, 1, px + (tmpw - wp) / 2, py);
}
// Redraw only the white box boundaries
// you do not notice the missing of the
// selected box
painter.drawLine(px, py, px1, py);
painter.drawLine(px, py1, px1, py1);
painter.drawLine(px, py, px, py1);
painter.drawLine(px1, py, px1, py1);
// Redraw the Current Character in black on Blue
UT_RGBColor colour(128, 128, 192);
painter.fillRect(colour, cx + m_areagc->tlu(1), cy + m_areagc->tlu(1), tmpw - m_areagc->tlu(1), tmph - m_areagc->tlu(1));
if(wc != GR_CW_ABSENT)
{
painter.drawChars(&c, 0, 1, cx + (tmpw - wc) / 2, cy);
}
}
int minMax(int depth, char color, int alpha, int beta, int total, int totalThreats)
{
color = (color == 'b') ? 'r' : 'b';
int lastRow = findLastRow(board);
int lastMoveValue = calculateAdjacentStep('b', board->lastColumn, lastRow);
int threatLevel = findGoodSquares(board->lastColumn, lastRow);
totalThreats += threatLevel;
//fprintf(stderr, "Value: %d\n", totalThreats);
if (color == 'r')
{
total -= (lastMoveValue);
// if (threatLevel > 0 && totalThreats < 2)
// {
total -= threatLevel * 25 - depth;
// }
// else if (threatLevel < 0 && totalThreats > -2)
// {
// total -= threatLevel * 50;
// }
//if (totalThreats > -1)
//{
//total -= threatLevel * 50;
//}
}
else
{
total += (lastMoveValue);
//if (threatLevel > 0 && totalThreats < 1)
//{
total += threatLevel * 25 + depth;
//}
//else if (threatLevel < 0 && totalThreats > -1)
//{
// total += threatLevel * 50;
// }
//if (totalThreats < 1)
//{
//total += threatLevel * 50;
///}
}
if (depth <= 0)
{
return calculatePosition(color, total);
}
Move* legalMoves = getLegalMoves(board, color);
if (legalMoves[0].colour == 'x')
{
return 0;
}
int currentValue;
int i;
for (i = 0; legalMoves[i].colour != 'x'; i++)
{
// if (difftime(time(NULL), startTime) > 170.0)
// {
// return -1000;
// }
if (legalMoves[i].colour == 'w')
{
if (legalMoves[i].column >= 0)
{
alpha = 1000 * legalMoves[i].column + depth;
}
else
{
alpha = 1000 * legalMoves[i].column - depth;
}
break;
}
makeMove(legalMoves[i]);
currentValue = -minMax(depth - 1, color, -beta, -alpha, -total, totalThreats);
undoMove();
if (currentValue >= beta)
{
return beta;
}
alpha = MAX(currentValue, alpha);
}
free(legalMoves);
return alpha;
}
void Map::read(PlayerCc::LaserProxy *lp, PlayerCc::FiducialProxy *fp, PlayerCc::Position2dProxy *pp) {
Point robpos = Point(WIDTH / 2 + pp->GetXPos() * PIXELS_PER_METER,
HEIGHT / 2 - pp->GetYPos() * PIXELS_PER_METER);
double robangle = pp->GetYaw();
double max = lp->GetMaxRange();
int lasers = lp->GetCount() - 1;
// FOOTPRINTS
footprints->push_back(robpos);
// FIDUCIAL
for (int i = 0; i < fp->GetCount(); i++) {
player_fiducial_item_t f = fp->GetFiducialItem(i);
Object object = Object();
// calculate the angle
double x = f.pose.px < 0 ? -f.pose.px : f.pose.px;
double y = f.pose.py < 0 ? -f.pose.py : f.pose.py;
double angle = x != 0 ? atan( y / x ) : M_PI_2;
angle = f.pose.px < 0 ? M_PI - angle : angle;
angle = f.pose.py < 0 ? robangle - angle : robangle + angle;
// calculate the position
Point p = calculatePosition(sqrt(x*x + y*y), angle);
p.x += robpos.x;
p.y += robpos.y;
// assign a color
if (f.id == 19) { // dead
object = Object(p, f.id, 1.1, 127, 127, 0);
} else if (f.id == 40) { // live
object = Object(p, f.id, 0.36, 0, 190, 0);
} else if (f.id == 105) { // fire
object = Object(p, f.id, 0.6, 255, 127, 0);
}
// check if object is already in the list
if (object.id != -1) {
bool exists = 0;
for (int j = 0; j < objects->size(); j++) {
if (objects->at(j).mayBeTheSame(object, PIXELS_PER_METER / 5)) {
exists = 1;
break;
}
}
if (!exists) {
objects->push_back(object);
#ifdef PRINT
if (f.id == 19) {
PRINT("BODY FOUND");
} else if (f.id == 40) {
PRINT("HUMAN FOUND");
} else if (f.id == 105) {
PRINT("FIRE DETECTED");
}
#endif
}
}
}
// LASERS
for (int l = 0; l < lasers; l++) {
double dist = lp->GetRange(l);
// = robangle + 2*M_PI*(l - lasers/2)/lasers;
double angle = robangle + l * 2 * M_PI / lasers - M_PI;
Point p = calculatePosition(dist, angle);
p.x += robpos.x;
p.y += robpos.y;
// set unwalkable on the grid and inform path finder about a wall
checkMap(p.x, p.y, WALLS_BOLDING_PX);
if (dist < max) {
for (int a = -WALLS_BOLDING_PX; a <= WALLS_BOLDING_PX; a++) {
for (int b = -WALLS_BOLDING_PX; b <= WALLS_BOLDING_PX; b++) {
grid[p.x + a][p.y + b].walkable = 0;
}
}
pathFinder->setWallDetectedAt(p.x, p.y);
}
// set seen
std::vector<Point> points = p.getPointsBetween(robpos);
for (int i = 0; i < (int) points.size() - (SEEN_BOLDING_PX + WALLS_BOLDING_PX + 1); i++) {
Point r = points.at(i);
for (int a = -SEEN_BOLDING_PX; a <= SEEN_BOLDING_PX; a++) {
for (int b = -SEEN_BOLDING_PX; b <= SEEN_BOLDING_PX; b++) {
grid[r.x + a][r.y + b].seen = 1;
}
}
// if (!grid[r.x][r.y].walkable) {
// /* Is not necessary for static environments
// * Slightly modifies existing walls
// * Margin is not removed, but it doesn't really matter
// * as long as return path may be created through the margin
// */
// grid[r.x][r.y].walkable = 1;
// }
}
}
};
void idle()
{
int amount=3;
while (packets->size() != 0 && amount > 0) {
amount--;
pthread_mutex_lock(&networkPacketsMutex);
packet pkt = packets->back();
particle p;
p.cyclesToKeepAround = totalCycles;
p.start = calculatePosition(pkt.sourceAddr, 2, 2);
p.dest = calculatePosition(pkt.destAddr, 2, 2);
if (is3d)
p.dest.y = -1;
p.curr = p.start;
if (!is3d)
p.speed = sqrt(pow(p.curr.x - p.dest.x, 2) + pow(p.curr.y - p.dest.y, 2)) / 30;
else
p.speed = sqrt(pow(p.curr.x - p.dest.x, 2) + pow(p.curr.y - p.dest.y, 2) + pow(p.curr.z - p.dest.z, 2)) / 30;
switch (pkt.type) {
case 0:
p.color[0] = 0.5;
p.color[1] = 0.0;
p.color[2] = 0.5;
break;
case 1:
p.color[0] = 0.0;
p.color[1] = 1.0;
p.color[2] = 0.0;
break;
case 2:
p.color[0] = 0.0;
p.color[1] = 0.0;
p.color[2] = 1.0;
break;
case 3:
p.color[0] = 0.0;
p.color[1] = 1.0;
p.color[2] = 1.0;
break;
case 4:
p.color[0] = 1.0;
p.color[1] = 0.0;
p.color[2] = 0.0;
break;
}
packets->pop_back();
pthread_mutex_unlock(&networkPacketsMutex);
if (p.speed > 0)
particles->push_back(p);
}
for (std::list<particle>::iterator i = particles->begin(); i != particles->end(); i++) {
particle p = *i;
bool keep = true;
if (p.curr.x == p.dest.x && p.curr.y == p.dest.y) {
p.cyclesToKeepAround--;
if (p.cyclesToKeepAround <= 0) {
keep = false;
i = particles->erase(i);
}
}
if (keep) {
if (!is3d)
p.curr = calculateCurrentPosition(p.curr, p.dest, p.speed);
else
p.curr = calculateCurrentPosition3d(p.curr, p.dest, p.start, p.speed);
i = particles->erase(i);
i = particles->insert(i, p);
}
}
particles->sort(sortParticle);
display();
}
void graphics::tick()
{
while (packets->size() != 0 && amount > 0) {
amount--;
pthread_mutex_lock(&networkPacketsMutex);
packet pkt = packets->back();
cubeTail p;
p.cyclesToKeepAround = totalCycles;
p.start = calculatePosition(pkt.sourceAddr, 2, 2);
p.dest = calculatePosition(pkt.destAddr, 2, 2);
if (is3d)
p.dest.y = -1;
p.curr = p.start;
if (!is3d)
p.speed = sqrt(pow(p.curr.x - p.dest.x, 2) + pow(p.curr.y - p.dest.y, 2)) / 30;
else
p.speed = sqrt(pow(p.curr.x - p.dest.x, 2) + pow(p.curr.y - p.dest.y, 2) + pow(p.curr.z - p.dest.z, 2)) / 30;
switch (pkt.type) {
case 0:
p.color[0] = 0.5;
p.color[1] = 0.0;
p.color[2] = 0.5;
break;
case 1:
p.color[0] = 0.0;
p.color[1] = 1.0;
p.color[2] = 0.0;
break;
case 2:
p.color[0] = 0.0;
p.color[1] = 0.0;
p.color[2] = 1.0;
break;
case 3:
p.color[0] = 0.0;
p.color[1] = 1.0;
p.color[2] = 1.0;
break;
case 4:
p.color[0] = 1.0;
p.color[1] = 0.0;
p.color[2] = 0.0;
break;
}
packets->pop_back();
pthread_mutex_unlock(&networkPacketsMutex);
if (p.speed > 0) {
p.cube = Cube(particleWidth / 2.0, p.curr, p.color[0], p.color[1], p.color[2]);
p.tail = Tail(p.curr, p.start, p.color[0], p.color[1], p.color[2]);
p.cube.generateBuffers();
p.tail.generateBuffers();
cubes.push_back(p);
}
}
for (std::list<cubeTail>::iterator i = cubes.begin(); i != cubes.end(); i++) {
cubeTail p = *i;
bool keep = true;
if (p.curr.x == p.dest.x && p.curr.y == p.dest.y) {
p.cyclesToKeepAround--;
if (p.cyclesToKeepAround <= 0) {
keep = false;
i = cubes.erase(i);
}
}
if (keep) {
if (!is3d)
p.curr = calculateCurrentPosition(p.curr, p.dest, p.speed);
else
p.curr = calculateCurrentPosition3d(p.curr, p.dest, p.start, p.speed);
i = cubes.erase(i);
i = cubes.insert(i, p);
}
}
cubes.sort(sortCubeTail);
for (std::list<cubeTail>::iterator i = cubes.begin(); i != cubes.end(); i++) {
cubeTail ct = *i;
Cube c = ct.cube;
Tail t = ct.tail;
double fade = (double)ct.cyclesToKeepAround / (double)totalCycles;
c.center = ct.curr;
c.render3(fade);
t.center = ct.curr;
t.render3(fade);
}
}
void Particle::calculatePhysics() {
calculateAcceleration();
calculateVelocity();
calculatePosition();
}