C++ CVector2::Rotate方法代码示例

 void CFootBotBaseGroundRotZOnlySensor::Update() {
    /*
     * We make the assumption that the robot is rotated only wrt to Z
     */
    /* Get robot position and orientation */
    const CVector3& cEntityPos = m_pcEmbodiedEntity->GetPosition();
    const CQuaternion& cEntityRot = m_pcEmbodiedEntity->GetOrientation();
    CRadians cRotZ, cRotY, cRotX;
    cEntityRot.ToEulerAngles(cRotZ, cRotY, cRotX);
    /* Set robot center */
    CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
    /* Position of sensor on the ground after rototranslation */
    CVector2 cSensorPos;
    /* Go through the sensors */
    for(UInt32 i = 0; i < m_tReadings.size(); ++i) {
       /* Calculate sensor position on the ground */
       cSensorPos = m_pcGroundSensorEntity->GetSensor(i+4).Offset;
       cSensorPos.Rotate(cRotZ);
       cSensorPos += cCenterPos;
       /* Get the color */
       const CColor& cColor = m_pcFloorEntity->GetColorAtPoint(cSensorPos.GetX(),
                                                               cSensorPos.GetY());
       /* Set the reading */
       m_tReadings[i].Value = cColor.ToGrayScale() / 255.0f;
       /* Apply noise to the sensor */
       if(m_bAddNoise) {
          m_tReadings[i].Value += m_pcRNG->Uniform(m_cNoiseRange);
       }
       /* Set the final reading */
       m_tReadings[i].Value = m_tReadings[i].Value < 0.5f ? 0.0f : 1.0f;
    }
 }

   void CFootBotMotorGroundSensor::Update() {
      /* We make the assumption that the foot-bot is rotated only wrt to Z */
      CFloorEntity& cFloorEntity = m_cSpace.GetFloorEntity();
      const CVector3& cEntityPos = GetEntity().GetEmbodiedEntity().GetPosition();
      const CQuaternion& cEntityRot = GetEntity().GetEmbodiedEntity().GetOrientation();
      CRadians cRotZ, cRotY, cRotX;
      cEntityRot.ToEulerAngles( cRotZ, cRotY, cRotX );
      CVector2 cCenterPos(cEntityPos.GetX(), cEntityPos.GetY());
      CVector2 cSensorPos;
      for(UInt32 i = 0; i < CCI_FootBotMotorGroundSensor::NUM_READINGS; ++i) {
         cSensorPos  = m_tReadings[i].Offset;
         cSensorPos.Rotate(cRotZ);
	 cSensorPos *= 0.01;
	 cSensorPos += cCenterPos;
         const CColor& cColor = cFloorEntity.GetColorAtPoint(cSensorPos.GetX(),cSensorPos.GetY());
         m_tReadings[i].Value = cColor.ToGrayScale()/255*FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.GetSpan();

	 if( m_fNoiseLevel > 0.0f ) {
	    AddNoise(i);
	 }

	 /* Normalize reading between 0 and 1, only if calibration has been performed */
         if( m_bCalibrated ) {
	    m_tReadings[i].Value = FOOTBOT_MOTOR_GROUND_SENSOR_READING_RANGE.NormalizeValue(m_tReadings[i].Value);
	 }
      }
   }

 void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center,
                                                    Real f_radius,
                                                    const CRadians& c_start_angle,
                                                    ESensorType e_type,
                                                    UInt32 un_num_sensors,
                                                    const SAnchor& s_anchor) {
    CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
    CRadians cAngle;
    CVector2 cOffset;
    for(UInt32 i = 0; i < un_num_sensors; ++i) {
       cAngle = c_start_angle + i * cSensorSpacing;
       cAngle.SignedNormalize();
       cOffset.Set(f_radius, 0.0f);
       cOffset.Rotate(cAngle);
       cOffset += c_center;
       AddSensor(cOffset, e_type, s_anchor);
    }
 }

  bool CKinematics2DEngine::CheckCollisions( const CKinematics2DCollisionCircle* pc_circle, const CKinematics2DCollisionRectangle* pc_rectangle ) {
    /* Rototranslate the plane so that the rectangle is axis aligned and centered in O */
    CVector2 center = pc_circle->GetPosition() - pc_rectangle->GetPosition();
    center.Rotate(pc_rectangle->GetOrientation() );
    center.Absolute();
    
    /* Find the Voronoi Region that the circle is in, exploiting the symmetries */
    CVector2 c_half_size   = pc_rectangle->GetHalfSize();
    Real     f_radius      = pc_circle->GetRadius();
    if( center.GetX() <= c_half_size.GetX() ) {
      /* The circle is in the top or bottom region */
      return (center.GetY() <= c_half_size.GetY() + f_radius);
    }
    
    if( center.GetY() <= c_half_size.GetY() ) {
      /* The circle is in the left or right region */
      return (center.GetX() <= c_half_size.GetX() + f_radius);
    }

    /* The circle is in one of the four corner regions */
    return (SquareDistance( c_half_size, center ) <= f_radius*f_radius);
  }