void Controller::runSnake()
{
double duration = ui->durationBox->value();
double currentTime, timeElapsed;
do
{
if (ui->modeTabs->currentIndex() == 0)
{
p = ui->simpgaits->getParams();
}
else
{
p = ui->advgaits->getParams();
}
timeElapsed = timer->elapsed();
currentTime = (double) (timeElapsed + msecElapsed)/1000;
d->setAllPosition(calculatePositions(currentTime));
d->getAllPosition(); // status packets
/* Manipulate position data here (e.g. return/save position, send to visualization) */
} while(runFlag && (duration == 0 || timeElapsed < duration*1000));
if (duration != 0)
{
ui->pauseButton->setChecked(1);
}
}
void SkeletonBlendedGeometry::skeletonUpdated(void)
{
produceChangedEvent();
if(getBaseGeometry())
{
calculatePositions();
}
}
void Goal::setBase(Point base)
{
m_location.screen = base;
valid = calculatePositions();
//valid = valid && check();
valid = check();
}
void SkeletonBlendedGeometry::drawPrimitives(DrawEnv *pEnv)
{
if(getBaseGeometry())
{
calculateJointTransform();
if(_NeedRecalc)
{
calculatePositions();
}
}
Inherited::drawPrimitives(pEnv);
}
Obstacle::Obstacle(Point position, double width, double height)
{
m_id = OBSTACLE;
m_location.screen = position;
m_size_on_screen = Vector2<double>(width, height);
// if(VisionConstants::DO_RADIAL_CORRECTION) {
// VisionBlackboard* vbb = VisionBlackboard::getInstance();
// Vector2<float> bottomcentre = Vector2<float>(position.x, position.y);
// bottomcentre = vbb->correctDistortion(bottomcentre);
// m_bottom_centre = Vector2<int>(bottomcentre.x, bottomcentre.y);
// }
//CALCULATE DISTANCE AND BEARING VALS
valid = calculatePositions();
//valid = valid && check();
valid = check();
}
const Point&
RangeSlider::setSize(float width, float height)
{
const Point& newSize = AbstractSliderBase::setSize(width, height);
float hdlW = std::max(round_pos(newSize.x * .05f), handleSize_);
lHandle_->setSize(hdlW, newSize.y - 2);
rHandle_->setSize(hdlW, newSize.y - 2);
int lx, rx;
calculatePositions(lx, rx);
lHandle_->setPos(lx, 1);
rHandle_->setPos(rx, 1);
updateRangeBar();
return newSize;
}
Goal::Goal(VFO_ID id, const Quad &corners)
{
m_id = id;
m_corners = corners;
m_location.screen = corners.getBottomCentre();
m_size_on_screen = Vector2<double>(corners.getAverageWidth(), corners.getAverageHeight());
// if(VisionConstants::DO_RADIAL_CORRECTION) {
// VisionBlackboard* vbb = VisionBlackboard::getInstance();
// Vector2<float> corr_bottom_centre = vbb->correctDistortion(Vector2<float>(m_bottom_centre.x, m_bottom_centre.y));
// m_bottom_centre.x = mathGeneral::roundNumberToInt(corr_bottom_centre.x);
// m_bottom_centre.y = mathGeneral::roundNumberToInt(corr_bottom_centre.y);
// }
//CALCULATE DISTANCE AND BEARING VALS
valid = calculatePositions();
//valid = valid && check();
valid = check();
}
void Controller::setInitialPosition()
{
if (ui->modeTabs->currentIndex() == 0)
{
p = ui->simpgaits->getParams();
}
else
{
p = ui->advgaits->getParams();
}
unsigned int numMotors = d->getNumMotors();
d->setAllSpeed(vector<int>(numMotors,SLOW_SPEED));
d->setAllPosition(calculatePositions((double)msecElapsed/1000));
d->getAllPosition(); // status packets
int checks = 0;
for (unsigned int ii = 0; ii < numMotors; ii++)
{
while(d->isMoving(ii) && checks++ < 10000);
}
d->setAllSpeed(vector<int>(numMotors,MAX_SPEED));
}
bool Associator::getAssociations( const Measurement& measurement, AssociatedMeasurement& associations, const int goal_node_i )
{
unassociated_points_ = measurement;
// Find a path through the graph starting from the associated nodes
PathFinder pathFinder( *graph_ptr_, associations.nodes );
pathFinder.findPath( goal_node_i, path_ );
// Calculate the positions of graph nodes on the path
AssociatedMeasurement path_positions = associations;
calculatePositions( *graph_ptr_, path_, path_positions );
associations.clear();
if ( !costCalculators_.size() )
{
std::cout << "\033[31m" << "[ASSOCIATOR] GetAssociations: No cost calculator modules available!" << "\033[0m" << std::endl;
}
for ( int i = 0; i < costCalculators_.size(); ++i )
{
// Reduce unassociated_points with any associated points and path_positions with the associated nodes before calling associate
for ( int j = 0; j < associations.nodes.size(); ++j )
{
path_positions.erase( path_positions.node_indices[ associations.nodes[j] ] );
unassociated_points_.points.erase( std::find(unassociated_points_.points.begin(), unassociated_points_.points.end(), associations.measurement.points[i] ) );
}
// Call the recursive association algorithms
associate( path_positions, unassociated_points_, associations, *costCalculators_[i], max_assoc_dists_[i]);
std::cout << associations.nodes.size() << " associations found after using costcalculator " << i << std::endl;
}
associated_ = true;
return true;
}
void SkeletonBlendedGeometry::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
if((whichField & BaseGeometryFieldMask) &&
getBaseGeometry() != NULL)
{
if(getBaseGeometry()->getTypes() != NULL)
{
setTypes(getBaseGeometry()->getTypes());
}
if(getBaseGeometry()->getLengths() != NULL)
{
setLengths(getBaseGeometry()->getLengths());
}
if(getBaseGeometry()->getPositions() != NULL)
{
GeoPropertyUnrecPtr Pos(getBaseGeometry()->getPositions()->clone());
setPositions(dynamic_pointer_cast<GeoVectorProperty>(Pos));
}
if(getBaseGeometry()->getNormals() != NULL)
{
GeoPropertyUnrecPtr Norm(getBaseGeometry()->getNormals()->clone());
setNormals(dynamic_pointer_cast<GeoVectorProperty>(Norm));
}
if(getBaseGeometry()->getColors() != NULL)
{
setColors(getBaseGeometry()->getColors());
}
if(getBaseGeometry()->getSecondaryColors() != NULL)
{
setSecondaryColors(getBaseGeometry()->getSecondaryColors());
}
if(getBaseGeometry()->getTexCoords() != NULL)
{
setTexCoords(getBaseGeometry()->getTexCoords());
}
if(getBaseGeometry()->getTexCoords1() != NULL)
{
setTexCoords1(getBaseGeometry()->getTexCoords1());
}
if(getBaseGeometry()->getTexCoords2() != NULL)
{
setTexCoords2(getBaseGeometry()->getTexCoords2());
}
if(getBaseGeometry()->getTexCoords3() != NULL)
{
setTexCoords3(getBaseGeometry()->getTexCoords3());
}
if(getBaseGeometry()->getTexCoords4() != NULL)
{
setTexCoords4(getBaseGeometry()->getTexCoords4());
}
if(getBaseGeometry()->getTexCoords5() != NULL)
{
setTexCoords5(getBaseGeometry()->getTexCoords5());
}
if(getBaseGeometry()->getTexCoords6() != NULL)
{
setTexCoords6(getBaseGeometry()->getTexCoords6());
}
if(getBaseGeometry()->getTexCoords7() != NULL)
{
setTexCoords7(getBaseGeometry()->getTexCoords7());
}
if(getBaseGeometry()->getIndices() != NULL)
{
setIndices(getBaseGeometry()->getIndices());
}
setMaterial(getBaseGeometry()->getMaterial());
}
if( (whichField & InternalJointsFieldMask) ||
(whichField & InternalWeightIndexesFieldMask) ||
(whichField & InternalWeightsFieldMask))
{
calculatePositions();
}
}
void SkeletonBlendedGeometry::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
//Do not respond to changes that have a Sync origin
if(origin & ChangedOrigin::Sync)
{
return;
}
if((whichField & BaseGeometryFieldMask) &&
getBaseGeometry() != NULL)
{
if(getBaseGeometry()->getTypes() != NULL)
{
setTypes(getBaseGeometry()->getTypes());
}
if(getBaseGeometry()->getLengths() != NULL)
{
setLengths(getBaseGeometry()->getLengths());
}
if(getBaseGeometry()->getPositions() != NULL)
{
GeoPropertyUnrecPtr Pos(getBaseGeometry()->getPositions()->clone());
setPositions(dynamic_pointer_cast<GeoVectorProperty>(Pos));
}
if(getBaseGeometry()->getNormals() != NULL)
{
GeoPropertyUnrecPtr Norm(getBaseGeometry()->getNormals()->clone());
setNormals(dynamic_pointer_cast<GeoVectorProperty>(Norm));
}
if(getBaseGeometry()->getColors() != NULL)
{
setColors(getBaseGeometry()->getColors());
}
if(getBaseGeometry()->getSecondaryColors() != NULL)
{
setSecondaryColors(getBaseGeometry()->getSecondaryColors());
}
if(getBaseGeometry()->getTexCoords() != NULL)
{
setTexCoords(getBaseGeometry()->getTexCoords());
}
if(getBaseGeometry()->getTexCoords1() != NULL)
{
setTexCoords1(getBaseGeometry()->getTexCoords1());
}
if(getBaseGeometry()->getTexCoords2() != NULL)
{
setTexCoords2(getBaseGeometry()->getTexCoords2());
}
if(getBaseGeometry()->getTexCoords3() != NULL)
{
setTexCoords3(getBaseGeometry()->getTexCoords3());
}
if(getBaseGeometry()->getTexCoords4() != NULL)
{
setTexCoords4(getBaseGeometry()->getTexCoords4());
}
if(getBaseGeometry()->getTexCoords5() != NULL)
{
setTexCoords5(getBaseGeometry()->getTexCoords5());
}
if(getBaseGeometry()->getTexCoords6() != NULL)
{
setTexCoords6(getBaseGeometry()->getTexCoords6());
}
if(getBaseGeometry()->getTexCoords7() != NULL)
{
setTexCoords7(getBaseGeometry()->getTexCoords7());
}
if(getBaseGeometry()->getIndices() != NULL)
{
setIndices(getBaseGeometry()->getIndices());
}
setMaterial(getBaseGeometry()->getMaterial());
}
if(whichField & InternalJointsFieldMask)
{
_JointPoseTransforms.resize(getNumJoints());
}
if( (whichField & BaseGeometryFieldMask) ||
(whichField & InternalJointsFieldMask) ||
(whichField & InternalWeightIndexesFieldMask) ||
(whichField & InternalWeightsFieldMask) ||
(whichField & BindTransformationFieldMask))
{
if(getNumJoints() > 0)
{
_JointPoseTransforms.resize(getNumJoints());
calculatePositions();
}
}
}
void SkeletonBlendedGeometry::changed(ConstFieldMaskArg whichField,
UInt32 origin,
BitVector details)
{
Inherited::changed(whichField, origin, details);
if((whichField & BaseGeometryFieldMask) &&
getBaseGeometry() != NULL)
{
if(getBaseGeometry()->getTypes() != NULL)
{
setTypes(getBaseGeometry()->getTypes());
}
if(getBaseGeometry()->getLengths() != NULL)
{
setLengths(getBaseGeometry()->getLengths());
}
if(getBaseGeometry()->getPositions() != NULL)
{
GeoPropertyUnrecPtr Pos(getBaseGeometry()->getPositions()->clone());
setPositions(dynamic_pointer_cast<GeoVectorProperty>(Pos));
}
if(getBaseGeometry()->getNormals() != NULL)
{
GeoPropertyUnrecPtr Norm(getBaseGeometry()->getNormals()->clone());
setNormals(dynamic_pointer_cast<GeoVectorProperty>(Norm));
}
if(getBaseGeometry()->getColors() != NULL)
{
setColors(getBaseGeometry()->getColors());
}
if(getBaseGeometry()->getSecondaryColors() != NULL)
{
setSecondaryColors(getBaseGeometry()->getSecondaryColors());
}
if(getBaseGeometry()->getTexCoords() != NULL)
{
setTexCoords(getBaseGeometry()->getTexCoords());
}
if(getBaseGeometry()->getTexCoords1() != NULL)
{
setTexCoords1(getBaseGeometry()->getTexCoords1());
}
if(getBaseGeometry()->getTexCoords2() != NULL)
{
setTexCoords2(getBaseGeometry()->getTexCoords2());
}
if(getBaseGeometry()->getTexCoords3() != NULL)
{
setTexCoords3(getBaseGeometry()->getTexCoords3());
}
if(getBaseGeometry()->getTexCoords4() != NULL)
{
setTexCoords4(getBaseGeometry()->getTexCoords4());
}
if(getBaseGeometry()->getTexCoords5() != NULL)
{
setTexCoords5(getBaseGeometry()->getTexCoords5());
}
if(getBaseGeometry()->getTexCoords6() != NULL)
{
setTexCoords6(getBaseGeometry()->getTexCoords6());
}
if(getBaseGeometry()->getTexCoords7() != NULL)
{
setTexCoords7(getBaseGeometry()->getTexCoords7());
}
if(getBaseGeometry()->getIndices() != NULL)
{
setIndices(getBaseGeometry()->getIndices());
}
setMaterial(getBaseGeometry()->getMaterial());
}
if((whichField & JointsFieldMask) ||
(whichField & PositionIndexesFieldMask) ||
(whichField & BlendAmountsFieldMask))
{
calculatePositions();
}
if(whichField & SkeletonsFieldMask)
{
for(std::vector<EventConnection>::iterator Itor(_SkeletonListenerConnections.begin()) ; Itor != _SkeletonListenerConnections.end() ; ++Itor)
{
Itor->disconnect();
}
_SkeletonListenerConnections.clear();
for(UInt32 i(0) ; i<getMFSkeletons()->size() ; ++i)
{
_SkeletonListenerConnections.push_back(getSkeletons(i)->addSkeletonListener(this));
}
}
}
void SkeletonBlendedGeometry::skeletonChanged(const SkeletonEventUnrecPtr e)
{
calculatePositions();
}
