/// setEndPose
void machineStatus::setEndPose( Point p) {
#ifdef MULTI_AXIS
endPose = Pose( p, Point(0,0,0) );
#else
endPose = Pose( p, Point(0,0,1) );
#endif
}
//Slot Unwrapper for internet datagram (vision)
void SimulatorCore::UnWrapVisionPacket(SSL_WrapperPacket iPacketTeam){
SSL_DetectionRobot lPacketRobotBlue;
SSL_DetectionRobot lPacketRobotYellow;
SSL_DetectionBall lPacketBall = iPacketTeam.detection().balls(0);
int lBlueSize = iPacketTeam.detection().robots_blue_size();
int lYellowSize = iPacketTeam.detection().robots_blue_size();
mBall->setPosition(lPacketBall.x(),lPacketBall.y());
for(int i = 0; i < lBlueSize; ++i){
lPacketRobotBlue = iPacketTeam.detection().robots_blue(i);
int RobotId = lPacketRobotBlue.robot_id();
if(RobotId > mNbPlayerPerTeam - 1){
break; //TODO clean this with dynamic player allocation
}
RobotModel* lRobotBlue = mBlueTeam->GetRobot(RobotId);
lRobotBlue->setPose(Pose(lPacketRobotBlue.x(),lPacketRobotBlue.y(),
lPacketRobotBlue.orientation()));
// TODO should be in another function
lRobotBlue->UpdateSimulationData();
}
for(int i = 0; i < lYellowSize; ++i){
lPacketRobotYellow = iPacketTeam.detection().robots_yellow(i);
int RobotId = lPacketRobotYellow.robot_id();
if(RobotId > mNbPlayerPerTeam - 1){
break; //TODO clean this with dynamic player allocation
}
RobotModel* lRobotYellow = mYellowTeam->GetRobot(RobotId);
lRobotYellow->setPose(Pose(lPacketRobotYellow.x(),lPacketRobotYellow.y(),
lPacketRobotYellow.orientation()));
}
this->sendCommandToGrSim(); //TODO not clear, send back the new command to grsim
}
void Sprite::move(double xshift, double yshift, bool repaintP){
Position oldorigin = pose.getOrigin();
pose = Pose(Pose(xshift,yshift),pose); // move in parent's frame
if(penIsDown){
Line l(oldorigin.x, oldorigin.y,
oldorigin.x+xshift, oldorigin.y+yshift);
//l.setColor(COLOR("green"));
l.imprint(false);
}
if(repaintP) repaint();
}
void MotionControl::drawMap(SDL_Surface* screen, int sx, int sy) const {
#if 0
//pixelRGBA(screen, sx, sy, 255, 255, 255, 255);
lineRGBA(screen, sx, sy, sx, sy-100, 255, 255, 0, 255);
lineRGBA(screen, sx, sy, sx+100, sy, 255, 255, 0, 255);
float pxl_per_m = 100;
filledCircleRGBA(screen, sx + lastPose.x * pxl_per_m, sy - lastPose.y * pxl_per_m, 5, 0, 255, 255, 255);
lineRGBA(screen, sx + lastPose.x * pxl_per_m, sy - lastPose.y * pxl_per_m, sx + (lastPose.x + cos(lastPose.theta)) * pxl_per_m, sy - (lastPose.y + sin(lastPose.theta)) * pxl_per_m, 255, 0, 0, 255);
PoseList poles{
Pose(3, 3.1, 0),
Pose(3, 2.3, 0),
Pose(3.2, 1.2, 0),
Pose(4, 1.2, 0),
Pose(2.2, 1.9, 0),
Pose(2.2, 1.1, 0),
Pose(1, 0.3, 0),
Pose(1, 1.1, 0),
Pose(1.5, 2.8, 0),
Pose(1.5, 2, 0)
};
for (auto it = poles.begin(); it != poles.end(); ++it) {
filledCircleRGBA(screen, sx + it->x * pxl_per_m, sy - it->y * pxl_per_m, 5, 255, 0, 0, 255);
}
for (auto it = waypoints.begin(); it != waypoints.end(); ++it) {
filledCircleRGBA(screen, sx + it->x * pxl_per_m, sy - it->y * pxl_per_m, 5, 0, 0, 255, 255);
}
auto last = midpoints.begin();
//cout << endl << "Starting le draw" << endl;
if (last != midpoints.end()) {
int i=1;
auto next = last;
lineRGBA(screen, sx + lastPose.x * pxl_per_m, sy - lastPose.y * pxl_per_m, sx + last->x * pxl_per_m, sy - last->y * pxl_per_m, 255, 255, 0, 255);
for (++next; ; ++next) {
float arrowx = cos(last->theta), arrowy = sin(last->theta);
lineRGBA(screen, sx + last->x * pxl_per_m, sy - last->y * pxl_per_m, sx + (last->x+arrowx) * pxl_per_m, sy - (last->y+arrowy) * pxl_per_m, 255-255*i/midpoints.size(), 255*i/midpoints.size(), 0, 255);
if (next == midpoints.end())
break;
lineRGBA(screen, sx + last->x * pxl_per_m, sy - last->y * pxl_per_m, sx + next->x * pxl_per_m, sy - next->y * pxl_per_m, 255, 255, 255, 255);
last = next;
i++;
}
}
#endif
}
/// reset machineStatus to reasonable defaults
void machineStatus::clearAll() {
F=S=0.0;
plane = CANON_PLANE_XY;
origin = Pose( Point(0,0,0), Point(0,0,0));
coolant.flood = false;
coolant.mist = false;
coolant.spindle = false;
endPose = startPose = Pose( Point(0,0,0), Point(0,0,1));
endDir = prevEndDir = Point(0,0,-1);
spindleStat = OFF;
myTool = -1;
motionType = NOT_DEFINED;
lastMotionWasTraverse = false;
}
bool Simulator::
Initialize()
{
for (size_t ic(0); ic < RobotClient::registry.size(); ++ic) {
RobotClient *client(RobotClient::registry.at(ic));
RobotServer *server(new RobotServer(client, *m_world));
if ( !client->Initialize(*server)) {
delete server;
delete client;
return false;
}
m_world->AddRobot(server);
m_robot.push_back(robot_s(server, client));
Frame const pose(client->m_initial_pose.x, client->m_initial_pose.y, client->m_initial_pose.theta);
server->InitializePose(pose);
client->SetPose(Pose(client->m_initial_pose.x, client->m_initial_pose.y, client->m_initial_pose.theta));
if (client->m_goals.size() > 0) {
client->SetGoal(m_timestep, client->m_goals[0]);
}
// This was a nice feature that might be worth resurrecting using fpplib
// if ( ! layout_file.empty())
// m_appwin.push_back(shared_ptr<AppWindow>(new AppWindow(rdesc[ii]->name, layout_file,
// 640, 480, this)));
}
UpdateAllSensors();
return true;
}
void CameraModel::insertBallWithNoise(SSL_WrapperPacket *iPacket,
BallData *iBall){
Pose lBallPose = Pose(iBall->getPosition().x,iBall->getPosition().y,0);
lBallPose = this->addNoise(lBallPose);
addBallToVisionPacket(iPacket,lBallPose);
}
void Joint::rotate(){
//not sure if this is how it works...
//local_pose.orientation = local_pose.orientation + rotate;
//PoseEuler temp = PoseEuler(vertex3(), vertex3(0.0, 0.0, angle));
// local_transformation = local_transformation * ( temp.getRotation()* angularVelocity);
Pose currentPose = path->update();
float x, y;
x = currentPose.position.getx();
y = currentPose.position.gety();
angle = -PI/2.0 + (float) (std::atan2(y, x) - std::atan2(1.0, 0.0));
//REALLY MEANS: angle = angularVelocity * dt + angle0;
//angle += angularVelocity;
//std::cout << "x:" << x << ", y:" << y << ", theta:" << angle << std::endl;
Pose temp = Pose(vertex3(), quaternion(angle, 0.0, 0.0));
//Pose temp = Pose(vertex3(), quaternion(0.0, angle, 0.0));
local_transformation = temp.getRotation();
//local_transformation = ( temp.getRotation() * angularVelocity);
//if(!isNull()){
// for(unsigned int i=0; i<children.size(); i++){
// this->children.at(i)->child->rotate(rotate);
// }
//}
}
Robot::Robot(){
id = 0;
is_our_team = true;
actPose = setPose = finalPose = Pose(0, 0, 0);
command = Command(0, 0, 0);
path_is_valid_yet = false;
}
//---------------------------------------------------------------------
Pose* Pose::clone(void) const
{
Pose* newPose = OGRE_NEW Pose(mTarget, mName);
newPose->mVertexOffsetMap = mVertexOffsetMap;
// Allow buffer to recreate itself, contents may change anyway
return newPose;
}
ExperimentalApp() : GLFWApp(1280, 800, "Glass Material App")
{
igm.reset(new gui::ImGuiManager(window));
gui::make_dark_theme();
//cubeTex = load_cubemap();
int width, height;
glfwGetWindowSize(window, &width, &height);
glViewport(0, 0, width, height);
// Debugging views for offscreen surfaces
uiSurface.bounds = {0, 0, (float) width, (float) height};
uiSurface.add_child( {{0.0000f, +10},{0, +10},{0.1667f, -10},{0.133f, +10}});
uiSurface.add_child( {{0.1667f, +10},{0, +10},{0.3334f, -10},{0.133f, +10}});
uiSurface.layout();
grid = RenderableGrid(1, 100, 100);
cameraController.set_camera(&camera);
camera.look_at({0, 2.5, -2.5}, {0, 2.0, 0});
lights[0] = {{0, 10, -10}, {0, 0, 1}};
lights[1] = {{0, 10, 10}, {0, 1, 0}};
Renderable reflectiveSphere = Renderable(make_sphere(2.f));
reflectiveSphere.pose = Pose(float4(0, 0, 0, 1), float3(0, 2, 0));
glassModels.push_back(std::move(reflectiveSphere));
Renderable debugAxis = Renderable(make_axis(), false, GL_LINES);
debugAxis.pose = Pose(float4(0, 0, 0, 1), float3(0, 1, 0));
regularModels.push_back(std::move(debugAxis));
Renderable cubeA = Renderable(make_cube());
cubeA.pose = Pose(make_rotation_quat_axis_angle({1, 0, 1}, ANVIL_PI / 3), float3(5, 0, 0));
regularModels.push_back(std::move(cubeA));
Renderable cubeB = Renderable(make_cube());
cubeB.pose = Pose(make_rotation_quat_axis_angle({1, 0, 1}, ANVIL_PI / 4), float3(-5, 0, 0));
regularModels.push_back(std::move(cubeB));
glassMaterialShader = make_watched_shader(shaderMonitor, "assets/shaders/glass_vert.glsl", "assets/shaders/glass_frag.glsl");
simpleShader = make_watched_shader(shaderMonitor, "assets/shaders/simple_vert.glsl", "assets/shaders/simple_frag.glsl");
cubeCamera.reset(new CubemapCamera({1024, 1024}));
gl_check_error(__FILE__, __LINE__);
}
Pose InputDevice::getPose(int channel) const {
auto pose = _poseStateMap.find(channel);
if (pose != _poseStateMap.end()) {
return (*pose).second;
} else {
return Pose();
}
}
void Joint::translate(const vertex3& movement){
//how much to move, or where to move to?
//simply add how much it moves to it's pose
Pose temp = Pose(movement, vertex3());
local_transformation = local_transformation * temp.translate_object();
//local_pose.position = local_pose.position + movement;
}
Pose Perception::getGlobalPose() const
{
if(master)
return Device::getGlobalPose();
else if(unit)
return unit->getPose();
return Pose(0,0);
}
void Map::loadPositions(std::istream &input) {
positions.clear();
std::string lineBuffer;
while (std::getline(input, lineBuffer)) {
positions.push_back(Pose(lineBuffer));
}
}
//--------------------------------------------------------------
void ofApp::setup(){
sender.setup("127.0.0.1", 8888);
kinect.open();
kinect.initBodySource();
// kinect.initColorSource();
kinect.initDepthSource();
// kinect.initBodyIndexSource();
kinect.getSensor()->get_CoordinateMapper(&m_pCoordinateMapper);
camera.setDistance(10);
ofSetWindowShape(1920, 1080);
CalcParams elbowR = { ShoulderRight, ElbowRight, WristRight };
CalcParams elbowL = { ShoulderLeft, ElbowLeft, WristLeft };
CalcParams zShoulderR = { HipRight, ShoulderRight, ElbowRight };
CalcParams zShoulderL = { HipLeft, ShoulderLeft, ElbowLeft };
jointCalcParams.insert( make_pair("elbowRight", elbowR) );
jointCalcParams.insert( make_pair("elbowLeft", elbowL) );
jointCalcParams.insert( make_pair("zShoulderRight", zShoulderR) );
jointCalcParams.insert( make_pair("zShoulderLeft", zShoulderL) );
// pose(ER, ER, zSR, zSL)
poses.insert( make_pair(2, Pose(180., 180., 100., 100.)) );
poses.insert( make_pair(1, Pose(180., 180., 15., 100.)) ); //f
poses.insert( make_pair(3, Pose(180., 165., 15., 180.)) ); // d
poses.insert( make_pair(4, Pose(180., 180., 15., 140.)) ); // e
poses.insert( make_pair(5, Pose(180., 180., 100., 15.)) ); // b
poses.insert( make_pair(6, Pose(180., 180., 140., 15.)) ); // c
poses.insert( make_pair(7, Pose(165., 180., 180., 100.)) ); // j
poses.insert( make_pair(8, Pose(180., 180., 60., 140.)) ); // l
poses.insert( make_pair(9, Pose(180., 165., 100., 180.)) );// p
poses.insert( make_pair(10, Pose(180., 180., 140., 140.)) );
showGUI = false;
gui.setup("Parameters");
gui.add(maxDistance.set("Max Distance", 5., 0., 10.));
gui.add(tol.set("Pose tolerance", 15., 0., 50.));
}
void Line::reset(double x1, double y1, double x2, double y2){
pose = Pose((x1+x2)/2,(y1+y2)/2);
double dx = (x1-x2)/2;
double dy = (y1-y2)/2;
vertex[0] = Position(dx,dy);
vertex[1] = Position(-dx,-dy);
thickness = 2;
show();
}
Pose WorldDesc::getObjPos(std::string name)
{
std::vector< std::pair< Pose, std::string > >::iterator ii=this->objectsPositions.begin();
for(;ii!=this->objectsPositions.end();ii++){
if((*ii).second==name){
return ii->first;
}
}
return Pose(0,0,0);
}
bool EvaluationModule::checkAngleToPass(Vector2D targetPosition, Pose currRobotPosition, double & angleToTarget) const{
angleToTarget = calculateAngleToTarget( currRobotPosition, Pose(targetPosition.x,targetPosition.y,0) );
double threshold = 0.017 * currRobotPosition.getPosition().distance(targetPosition);
//LOG_INFO(this->log,"set threshold to "<<threshold);
if( fabs( angleToTarget ) < threshold ){//1stopien
return true;
}
return false;
}
void TreeOptimizer2::computePreconditioner(){
gamma[0] = gamma[1] = gamma[2] = numeric_limits<double>::max();
for (uint i=0; i<M.size(); i++)
M[i]=Pose(0.,0.,0.);
int edgeCount=0;
for (EdgeSet::iterator it=sortedEdges->begin(); it!=sortedEdges->end(); it++){
edgeCount++;
if (! (edgeCount%10000))
DEBUG(1) << "m";
Edge* e=*it;
Transformation t=e->transformation;
InformationMatrix S=e->informationMatrix;
InformationMatrix R;
R.values[0][0]=t.rotationMatrix[0][0];
R.values[0][1]=t.rotationMatrix[0][1];
R.values[0][2]=0;
R.values[1][0]=t.rotationMatrix[1][0];
R.values[1][1]=t.rotationMatrix[1][1];
R.values[1][2]=0;
R.values[2][0]=0;
R.values[2][1]=0;
R.values[2][2]=1;
InformationMatrix W =R*S*R.transpose();
Vertex* top=e->top;
for (int dir=0; dir<2; dir++){
Vertex* n = (dir==0)? e->v1 : e->v2;
while (n!=top){
uint i=n->id;
M[i].values[0]+=W.values[0][0];
M[i].values[1]+=W.values[1][1];
M[i].values[2]+=W.values[2][2];
gamma[0]=gamma[0]<W.values[0][0]?gamma[0]:W.values[0][0];
gamma[1]=gamma[1]<W.values[1][1]?gamma[1]:W.values[1][1];
gamma[2]=gamma[2]<W.values[2][2]?gamma[2]:W.values[2][2];
n=n->parent;
}
}
}
if (verboseLevel>1){
for (uint i=0; i<M.size(); i++){
cerr << "M[" << i << "]=" << M[i].x() << " " << M[i].y() << " " << M[i].theta() <<endl;
}
}
}
Task::status Rotate::run(void * arg, int steps){
GameStatePtr currGameState( new GameState );
double lastSimTime=0;
double currSimTime=0;
currSimTime=video.updateGameState(currGameState);
//biezaca pozycja robota
Pose currRobotPose = (*currGameState).getRobotPos( robot->getRobotID() );
if(this->targetRobotId != Robot::unknown)
targetPosition = (*currGameState).getRobotPos(this->targetRobotId).getPosition();
while( !this->stopTask && (steps--)!=0 ){
if( lastSimTime < ( currSimTime=video.updateGameState(currGameState) ) ){
lastSimTime = currSimTime;
currRobotPose=currGameState->getRobotPos( robot->getRobotID() );
bool haveBall = false;
double angleToTarget = 0;
double threshold = 0.017;
if(this->predicates & Task::pass){
bool canPass=this->evaluationModule.checkAngleToPass(targetPosition, currRobotPose, angleToTarget);
threshold = 0.017 * currRobotPose.getPosition().distance(this->targetPosition);
LOG_INFO(this->log,"set threshold to "<<threshold);
if( canPass ){
this->robot->stop();
LOG_INFO(log,"Rotation to pass OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget);
return Task::ok;
}
}
else{
angleToTarget = calculateAngleToTarget( currRobotPose, Pose(targetPosition.x,targetPosition.y,0) );
double threshold = 0.017; //1 stopien
if( fabs( angleToTarget ) < threshold ){
this->robot->stop();
LOG_INFO(log,"Rotation OK robot position "<<currRobotPose<<" target "<<targetPosition<<" angle to target "<<angleToTarget);
return Task::ok;
}
}
double w = robot->calculateAngularVel( currRobotPose, targetPosition, currGameState->getSimTime(), haveBall );
LOG_INFO(log,"move robot from"<<currRobotPose<<" to "<<targetPosition<<" setVel w "<<w<<" angle to target "<<angleToTarget);
robot->setRelativeSpeed( Vector2D(0,0), w );
}
usleep(100);
}
if(this->stopTask)
return Task::ok;
return Task::not_completed;
}
Pose Goodrich::calcResult(int id, Pose goal, bool is_last){
this->id = id;
this->goal = goal;
this->is_last = is_last;
result = Pose(0, 0, 0);
attractiveForce();
repulsiveForceRobotObjects();
repulsiveForceRobotRobot();
return result;
}
void Line::paint(Pose *p){
if(visible){
XPoint xpt[2];
if(p){
Pose r = Pose(*p,pose);
r.compute_res_vertex(xpt, vertex, 2);
}
else pose.compute_res_vertex(xpt, vertex, 2);
drawLine(xpt[0], xpt[1], color, (int) thickness);
}
}
PID::PID(){
proportionalL = 0.40; //p1 = 0.0004
derivativeL = 0.0;
integrativeL = 0.0;
proportionalA = 0.80; //p2 = 0.8000
derivativeA = 0.0;
integrativeA = 0.0;
command = Command(0, 0, 0);
actPose = setPose = Pose(0, 0, 0);
}
void TreeOptimizer3::initializeOnlineOptimization(EdgeCompareMode mode){
edgeCompareMode=mode;
// compute the size of the preconditioning matrix
clear();
Vertex* v0=addVertex(0,Pose(0,0,0,0,0,0));
root=v0;
v0->parameters=Transformation(v0->pose);
v0->parentEdge=0;
v0->parent=0;
v0->level=0;
v0->transformation=Transformation(TreePoseGraph3::Pose(0,0,0,0,0,0));
}
// warning mixed line endings in pose files will screw you up
// windows seems to handle ok but in linux, the mixed line endings
// will have the affect of appearing to take angles from subsequent
// poses and mashing them into one pose ... eg multiple seperate lines
// are condensed into one with the most recent angles overwriting earlier angles
void Kitten::readPoseFile(char const *path) {
// Open the file.
std::ifstream fh;
fh.open(path);
std::vector<double> angles;
while (fh.good()) {
double x, y, z;
angles.clear();
// Read a line from the file.
std::string line;
getline(fh, line);
// Skip empty lines, or comments.
if (!line.size() || line[0] == '#') {
continue;
}
// Push the line into a stringstream so we can easily pull out doubles.
std::stringstream ss;
ss << line;
// Read translation coordinates.
ss >> x;
ss >> y;
ss >> z;
// Read angles while there is still stuff to read.
while (ss.good()) {
double a;
ss >> a;
angles.push_back(a);
}
int missing = NUM_ANGLES- angles.size();
for (int i = 0; i< missing;i++){
angles.push_back(0);
}
for (int i =0;i<angles.size();i++){
if (angles[i]!=0){
std::cout << "pose: " << poses_.size() << " angleno: " << i
<< " " << anglearray[i] <<" angle: " << angles[i] << std::endl;
}
}
poses_.push_back(Pose(x, y, z, angles));
}
}
Pose Robot::getPose() const {
double x=0, y=0, z=0;
std::vector<double> angles(NUM_ANGLES);
// If we don't have any poses then return an empty pose.
if (!poses_.size()) {
return Pose(x, y, z, angles);
}
// If we only have one pose, then return it.
if (poses_.size() == 1) {
return poses_[0];
}
// Return the current pose.
// NOTE: poseIndex is double, NOT an int!!! Fractional values represent
// interpolation between neighbouring poses. Eg: 2.5 would be halfway
// between the 3rd and 4th pose.
int wholeIndex = int(floor(poseIndex_));
double indexDiff = poseIndex_ - wholeIndex;
Pose tempPose(poses_[wholeIndex].x_,
poses_[wholeIndex].y_,
poses_[wholeIndex].z_,
angles);
double angleDiff = 0;
int nextIndex = 0;
if (poseIndex_ < poses_.size() -1)
{
nextIndex = int(floor(poseIndex_)) + 1;
}
//Translation diff
double diff = (poses_[nextIndex].x_ - poses_[int(floor(poseIndex_))].x_) * indexDiff;
tempPose.x_ = tempPose.x_ + diff;
diff = (poses_[nextIndex].y_ - poses_[int(floor(poseIndex_))].y_ ) * indexDiff;
tempPose.y_ = tempPose.y_ + diff;
diff = (poses_[nextIndex].z_ - poses_[int(floor(poseIndex_))].z_ ) * indexDiff;
tempPose.z_ = tempPose.z_ + diff;
//Angles diff
for (int i = NUM_ANGLES; i >= 0; i--)
{
angleDiff = (poses_[nextIndex].angles_[i] - poses_[int(floor(poseIndex_))].angles_[i]) * indexDiff;
tempPose.angles_[i] = poses_[int(floor(poseIndex_))].angles_[i] + angleDiff;
}
return tempPose;
// @@@@@ Replace the above return statement with your own code to implement
// @@@@@ linear interpolation between poses.
}
Drone::Drone(int txPin, int rxPin) {
_serialIO = SerialIO(txPin, rxPin);
_callback = Callback();
_incomingPacketReader = IncomingPacketReader(&_serialIO);
_vitals = Vitals(&_serialIO, &_incomingPacketReader, &_callback);
_responseHandler = ResponseHandler(&_serialIO, &_incomingPacketReader, &_callback, &_vitals);
_rc = RC(&_serialIO, &_callback, &_incomingPacketReader);
_gpio = GPIO(&_serialIO, &_callback, &_incomingPacketReader);
_i2c = I2C(&_serialIO, &_callback, &_incomingPacketReader);
_pose = Pose(&_serialIO, &_callback, &_incomingPacketReader);
_autopilot = Autopilot(&_serialIO, &_callback, &_incomingPacketReader);
_transmitterSupport = TransmitterSupport(&_serialIO, &_callback, &_incomingPacketReader);
}
void Turret::update(float dt)
{
effects.setPose(master->getMountPose(mount)*Pose(vec2f(0,0),getAngle()*3.1415/180.0));
effects.update(dt);
angle+=(_direction*velocity*dt);
if(angle<-180)
angle+=360;
if(angle> 180)
angle-=360;
if(angle>dimensions)
angle=dimensions;
if(angle<-dimensions)
angle=-dimensions;
}
Pose interpolate(quint64 time)
{
const quint64 sz=mPoses.size();
if(sz<=0) {
return Pose(0);
}
const quint64 b=before(time);
const quint64 a=after(time);
if(a==b) {
return mPoses[a];
}
const qreal alpha=(qreal)(time-b)/(qreal)(a-b);
Pose out=mPoses[b];
out.mix(mPoses[a], alpha);
}