C++ Motion类代码示例

void SceneCamera::BrainOrder(const State& state)
{
    if (state.GetStateType() == (int)State::TYPE::PAUSE )
    {
        if (state.IsNew())
        {
            SwitchCameraMode(CONTROL_TYPE::CHARACTER);
        }
    }
    
    if (state.GetStateType() == (int)State::TYPE::MOTION )
    {
        if (state.IsNew())
        {
            SwitchCameraMode(CONTROL_TYPE::CHARACTER);
        }
        
        Motion* motion = (Motion*)&state;
        
        cameraControl.Move(motion->GetSpeed(), motion->GetDir());
    }
    
     if (state.GetStateType() == (int)State::TYPE::SPACE_LOC ) {
         
         SpaceLocation* space_loc = (SpaceLocation*)&state;
         
         if (state.IsNew())
         {
             SwitchCameraMode(CONTROL_TYPE::AUTO_CONTROL);
         }
  
            ((CameraAutoControl*)control[currentType])->SetPosition(space_loc->GetPos());
            ((CameraAutoControl*) control[currentType])->SetDirection(space_loc->GetDir());
     }
}

int main()
{
  Motion motion;

  //motion.moveRight(1000);

  //usleep(5000000);

  motion.moveForward(1000);
  motion.moveForward(200);

  //usleep(5000000);

  //motion.spinACW(90);

  //usleep(5000000);

  //motion.spinCW(90);

  //motion.moveQ2(1000);

  //motion.moveQ1(500);
  //usleep(5000000);
  //motion.moveQ2(500);
  //usleep(5000000);
  //motion.moveQ3(500);
  //usleep(5000000);
  //motion.moveQ4(500);
  //usleep(5000000);
}

void ompl::geometric::FMT::sampleFree(const base::PlannerTerminationCondition &ptc)
{
    unsigned int nodeCount = 0;
    unsigned int sampleAttempts = 0;
    Motion *motion = new Motion(si_);

    // Sample numSamples_ number of nodes from the free configuration space
    while (nodeCount < numSamples_ && !ptc)
    {
        sampler_->sampleUniform(motion->getState());
        sampleAttempts++;

        bool collision_free = si_->isValid(motion->getState());

        if (collision_free)
        {
            nodeCount++;
            nn_->add(motion);
            motion = new Motion(si_);
        } // If collision free
    } // While nodeCount < numSamples
    si_->freeState(motion->getState());
    delete motion;

    // 95% confidence limit for an upper bound for the true free space volume
    freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) * si_->getStateSpace()->getMeasure();
}

void Motion::playMotions() {
  vector<Motion *>::iterator motionIt;
  for (motionIt = cMotions.begin() ; motionIt != cMotions.end(); ++motionIt) {
    Motion *motion = *motionIt;
    motion->playStep();
  }
}

 inline Motion operator^( const Motion& m1, const MotionRevoluteUnaligned & m2)
 {
   /* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
   const Motion::Vector3& v1 = m1.linear();
   const Motion::Vector3& w1 = m1.angular();
   const Motion::Vector3& w2 = m2.axis * m2.w ;
   return Motion( v1.cross(w2),w1.cross(w2));
 }

	void Pose::set(const Motion &motion, MotionChannel::PosType custom_postype, MotionChannel::IsOpen custom_isopen, float keytime)
	{
		for(Motion::const_iterator i = motion.begin(); i != motion.end(); ++i) {
			for(MotionChannelVector::const_iterator j = i->second.begin(); j != i->second.end(); ++j) {
				set(i->first.c_str(), **j, custom_postype, custom_isopen, keytime);
			}
		}
	}

 friend Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v)
 {
   EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
   Motion result (Motion::Zero ());
   result.linear ().template head<2> () = v.template topRows<2> ();
   result.angular ().template tail<1> () = v.template bottomRows<1> ();
   return result;
 }

bool  MotionManager::saveMotion(const std::string &animName, const std::string& fileName) //const
{
	 Motion* anim = getMotion(animName);
	if(anim)
	{
		anim->writeToFile(fileName);
		return true;
	}
	return false;
}

    friend Motion operator+ (const MotionPlanar & m1, const Motion & m2)
    {
      Motion result (m2);
      result.linear ()[0] += m1.x_dot_;
      result.linear ()[1] += m1.y_dot_;

      result.angular ()[2] += m1.theta_dot_;

      return result;
    }

int main(){

	int width = 800;
	int height = 600;

	//Game Window
	static Window window("Interstellar Explorer", width, height);
	glClearColor(0.0f, 0.0f, 0.0f, 0.0f);

	Motion motion;
	Orbit orb1;  //Also this
	Orbit orb2;

	// Construction
	PlayerObject player("Imgs/ship.png"); //Set filename to empty quotes to leave player as polygon
	vector<CircleObject> test = loadPlanets(test, "level.txt");

	//OpenGl Coordinate Grid Setup
	glViewport(0, 0, width, height);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(-width / 2, width / 2, -height / 2, height / 2, 0, 1);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	while (!window.closed()){
		window.clear();

		checkForInput(&window, &motion);

		// Translation
		glPushMatrix();
		motion.applySpeed();
		for (unsigned int i = 0; i < test.size(); i++){
			test[i].Draw();
		}
		glPopMatrix();

		// Rotation
		glPushMatrix();
		motion.applyRotation();
		player.Draw();
		glPopMatrix();

		//Orbits
		glPushMatrix();
		orb1.orbit(test);
		glPopMatrix();

		window.update();
		Sleep(0.5); //Controls how fast the game loop runs
	}

	return 0;
}

// Update with new frames
void LKTracker::Update(cv::Mat& frame, cv::Mat& next)
{
	MotionVector::iterator iter;

	// Update all tracked region
	for(iter = this->regions.begin(); iter != this->regions.end(); ++iter)
	{
		Motion *motionRegion = *iter;
		motionRegion->Update(frame, next);
	}
}

  Motion operator^ (const Motion & m1, const JointPlanar::MotionPlanar & m2)
  {
    Motion result;

    const Motion::Vector3 & m1_t = m1.linear();
    const Motion::Vector3 & m1_w = m1.angular();

    result.angular () << m1_w(1) * m2.theta_dot_, - m1_w(0) * m2.theta_dot_, 0.;
    result.linear () << m1_t(1) * m2.theta_dot_ - m1_w(2) * m2.y_dot_, - m1_t(0) * m2.theta_dot_ + m1_w(2) * m2.x_dot_, m1_w(0) * m2.y_dot_ - m1_w(1) * m2.x_dot_;

    return result;
  }

std::string MotionManager::loadMotion(const std::string& filename)
{
	MatrixXd mat = loadData(filename);
	if (mat.rows() > 0)
	{
		int pos = filename.find_last_of('\\') + 1;
		std::string motionName = filename.substr(pos);
		Motion *anim = new Motion(mat, motionName);
			
		mMotion.push_back(anim);
 		return anim->getName();
 	}
	return "";
}

void ompl::geometric::FMT::traceSolutionPathThroughTree(Motion *goalMotion)
{
    std::vector<Motion*> mpath;
    Motion *solution = goalMotion;

    // Construct the solution path
    while (solution != nullptr)
    {
        mpath.push_back(solution);
        solution = solution->getParent();
    }

    // Set the solution path
    PathGeometric *path = new PathGeometric(si_);
    int mPathSize = mpath.size();
    for (int i = mPathSize - 1 ; i >= 0 ; --i)
        path->append(mpath[i]->getState());
    pdef_->addSolutionPath(base::PathPtr(path), false, -1.0, getName());
}

void TileEntity::Update() {
	Entity::Update();
	
    // update the fire flicker
    if ( IsOnFire() ) {
        fireDurationCounter++;
        if ( !(fireFlickerVisual->IsPlayingAnimation()) ) {
            fireFlickerVisual->PlayAnimation( fireFlickerAnim );
        }
    }

    // update the motion, if we have one
    if ( !motionQueue.empty() ) {

        Motion * curMotion = motionQueue.front();

        curMotion->Update();

        if ( curMotion->IsDone() ) {
            motionQueue.pop_front();
            pos->SetToClosestTile();
            
            if ( sourceTileLoc != -1 ) {
                // grab current chamber and remove from location
                Chamber * curChamber = (ChamberManager::GetInstance()).GetCurrentChamber();
                curChamber->UnregisterTileEntityInTile( this, sourceTileLoc );
                
                // Call onExtited tile on all tile entities in previous tile
                curChamber->OnEntityExitedTile( this, sourceTileLoc );
                
                // Call onEntered tile on all tile entities except for this one in current tile
                curChamber->OnEntityEnteredTile( this, Chamber::GetTileNumFromPos( pos->GetTileX(), pos->GetTileY() ) );

                // Call onMoveCompleted on self
                OnMoveCompleted( curMotion );
            }

            delete curMotion;
        }

    }
}