Ejemplo n.º 1
0
Quaternion Quaternion::mul(Vector3f r)
{
    float w_ = -x * r.getX() - y * r.getY() - z * r.getZ();
    float x_ =  w * r.getX() + y * r.getZ() - z * r.getY();
    float y_ =  w * r.getY() + z * r.getX() - x * r.getZ();
    float z_ =  w * r.getZ() + x * r.getY() - y * r.getX();

    Quaternion *product =  new Quaternion(x_, y_, z_, w_);
    return *product;
}
Ejemplo n.º 2
0
//----------------------------------------------------------------------------
void GelatinCube::CreateSprings ()
{
    // The inner 4-by-4-by-4 particles are used as the control points of a
    // B-spline volume.  The outer layer of particles are immovable to
    // prevent the cuboid from collapsing into itself.
    int iSlices = 6;
    int iRows = 6;
    int iCols = 6;

    // Viscous forces applied.  If you set viscosity to zero, the cuboid
    // wiggles indefinitely since there is no dissipation of energy.  If
    // the viscosity is set to a positive value, the oscillations eventually
    // stop.  The length of time to steady state is inversely proportional
    // to the viscosity.
#ifdef _DEBUG
    float fStep = 0.1f;
#else
    float fStep = 0.01f;  // simulation needs to run slower in release mode
#endif
    float fViscosity = 0.01f;
    m_pkModule = new PhysicsModule(iSlices,iRows,iCols,fStep,fViscosity);

    // The initial cuboid is axis-aligned.  The outer shell is immovable.
    // All other masses are constant.
    float fSFactor = 1.0f/(float)(iSlices-1);
    float fRFactor = 1.0f/(float)(iRows-1);
    float fCFactor = 1.0f/(float)(iCols-1);
    int iSlice, iRow, iCol;
    for (iSlice = 0; iSlice < iSlices; iSlice++)
    {
        for (iRow = 0; iRow < iRows; iRow++)
        {
            for (iCol = 0; iCol < iCols; iCol++)
            {
                m_pkModule->Position(iSlice,iRow,iCol) =
                    Vector3f(iCol*fCFactor,iRow*fRFactor,iSlice*fSFactor);

                if ( 1 <= iSlice && iSlice < iSlices-1
                &&   1 <= iRow && iRow < iRows-1
                &&   1 <= iCol && iCol < iCols-1 )
                {
                    m_pkModule->SetMass(iSlice,iRow,iCol,1.0f);
                    m_pkModule->Velocity(iSlice,iRow,iCol) =
                        0.1f*Vector3f(Mathf::SymmetricRandom(),
                        Mathf::SymmetricRandom(),Mathf::SymmetricRandom());
                }
                else
                {
                    m_pkModule->SetMass(iSlice,iRow,iCol,Mathf::MAX_REAL);
                    m_pkModule->Velocity(iSlice,iRow,iCol) = Vector3f::ZERO;
                }
            }
        }
    }

    // springs are at rest in the initial configuration
    const float fConstant = 10.0f;
    Vector3f kDiff;

    for (iSlice = 0; iSlice < iSlices-1; iSlice++)
    {
        for (iRow = 0; iRow < iRows; iRow++)
        {
            for (iCol = 0; iCol < iCols; iCol++)
            {
                m_pkModule->ConstantS(iSlice,iRow,iCol) = fConstant;
                kDiff = m_pkModule->Position(iSlice+1,iRow,iCol) -
                    m_pkModule->Position(iSlice,iRow,iCol);
                m_pkModule->LengthS(iSlice,iRow,iCol) = kDiff.Length();
            }
        }
    }

    for (iSlice = 0; iSlice < iSlices; iSlice++)
    {
        for (iRow = 0; iRow < iRows-1; iRow++)
        {
            for (iCol = 0; iCol < iCols; iCol++)
            {
                m_pkModule->ConstantR(iSlice,iRow,iCol) = fConstant;
                kDiff = m_pkModule->Position(iSlice,iRow+1,iCol) -
                    m_pkModule->Position(iSlice,iRow,iCol);
                m_pkModule->LengthR(iSlice,iRow,iCol) = kDiff.Length();
            }
        }
    }

    for (iSlice = 0; iSlice < iSlices; iSlice++)
    {
        for (iRow = 0; iRow < iRows; iRow++)
        {
            for (iCol = 0; iCol < iCols-1; iCol++)
            {
                m_pkModule->ConstantC(iSlice,iRow,iCol) = fConstant;
                kDiff = m_pkModule->Position(iSlice,iRow,iCol+1) -
                    m_pkModule->Position(iSlice,iRow,iCol);
                m_pkModule->LengthC(iSlice,iRow,iCol) = kDiff.Length();
            }
        }
    }
}
Ejemplo n.º 3
0
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    Matrix3f temp_dcm = _dcm_matrix;
    Vector3f velocity;
    uint32_t last_correction_time;

    // perform yaw drift correction if we have a new yaw reference
    // vector
    drift_correction_yaw();

    // apply trim
    temp_dcm.rotateXY(_trim);

    // rotate accelerometer values into the earth frame
    _accel_ef = temp_dcm * _ins.get_accel();

    // integrate the accel vector in the earth frame between GPS readings
    _ra_sum += _accel_ef * deltat;

    // keep a sum of the deltat values, so we know how much time
    // we have integrated over
    _ra_deltat += deltat;
    num_sat=_gps->num_sats;

    if (!have_gps() ||
            _gps->status() < GPS::GPS_OK_FIX_3D ||
            _gps->num_sats < _gps_minsats) {
        // no GPS, or not a good lock. From experience we need at
        // least 6 satellites to get a really reliable velocity number
        // from the GPS.
        //
        // As a fallback we use the fixed wing acceleration correction
        // if we have an airspeed estimate (which we only have if
        // _fly_forward is set), otherwise no correction
        if (_ra_deltat < 0.2f) {
            // not enough time has accumulated
            return;
        }
        float airspeed;
        if (_airspeed && _airspeed->use()) {
            airspeed = _airspeed->get_airspeed();
        } else {
            airspeed = _last_airspeed;
        }
        // use airspeed to estimate our ground velocity in
        // earth frame by subtracting the wind
        velocity = _dcm_matrix.colx() * airspeed;

        // add in wind estimate
        velocity += _wind;

        last_correction_time = hal.scheduler->millis();
        _have_gps_lock = false;
    } else {
        if (_gps->last_fix_time == _ra_sum_start) {
            // we don't have a new GPS fix - nothing more to do
            return;
        }
        velocity = Vector3f(_gps->velocity_north(), _gps->velocity_east(), _gps->velocity_down());
        last_correction_time = _gps->last_fix_time;
        if (_have_gps_lock == false) {
            // if we didn't have GPS lock in the last drift
            // correction interval then set the velocities equal
            _last_velocity = velocity;
        }
        _have_gps_lock = true;

        // keep last airspeed estimate for dead-reckoning purposes
        Vector3f airspeed = velocity - _wind;
        airspeed.z = 0;
        _last_airspeed = airspeed.length();
    }

    if (have_gps()) {
        // use GPS for positioning with any fix, even a 2D fix
        _last_lat = _gps->latitude;
        _last_lng = _gps->longitude;
        _position_offset_north = 0;
        _position_offset_east = 0;

        // once we have a single GPS lock, we can update using
        // dead-reckoning from then on
        _have_position = true;
    } else {
        // update dead-reckoning position estimate
        _position_offset_north += velocity.x * _ra_deltat;
        _position_offset_east  += velocity.y * _ra_deltat;
    }

    // see if this is our first time through - in which case we
    // just setup the start times and return
    if (_ra_sum_start == 0) {
        _ra_sum_start = last_correction_time;
        _last_velocity = velocity;
        return;
    }

    // equation 9: get the corrected acceleration vector in earth frame. Units
    // are m/s/s
    Vector3f GA_e;
    GA_e = Vector3f(0, 0, -1.0f);

    bool using_gps_corrections = false;
    if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
        float v_scale = gps_gain.get()/(_ra_deltat*GRAVITY_MSS);
        Vector3f vdelta = (velocity - _last_velocity) * v_scale;
        GA_e += vdelta;
        GA_e.normalize();
        if (GA_e.is_inf()) {
            // wait for some non-zero acceleration information
            return;
        }
        using_gps_corrections = true;
    }

    // calculate the error term in earth frame.
    _ra_sum /= (_ra_deltat * GRAVITY_MSS);

    // get the delayed ra_sum to match the GPS lag
    Vector3f GA_b;
    if (using_gps_corrections) {
        GA_b = ra_delayed(_ra_sum);
    } else {
        GA_b = _ra_sum;
    }
    GA_b.normalize();
    if (GA_b.is_inf()) {
        // wait for some non-zero acceleration information
        return;
    }

    Vector3f error = GA_b % GA_e;

#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
    // step 2 calculate earth_error_Z
    float earth_error_Z = error.z;

    // equation 10
    float tilt = pythagorous2(GA_e.x, GA_e.y);

    // equation 11
    float theta = atan2f(GA_b.y, GA_b.x);

    // equation 12
    Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);

    // step 6
    error = GA_b % GA_e2;
    error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION

    // to reduce the impact of two competing yaw controllers, we
    // reduce the impact of the gps/accelerometers on yaw when we are
    // flat, but still allow for yaw correction using the
    // accelerometers at high roll angles as long as we have a GPS
    if (use_compass()) {
        if (have_gps() && gps_gain == 1.0f) {
            error.z *= sinf(fabsf(roll));
        } else {
            error.z = 0;
        }
    }

    // if ins is unhealthy then stop attitude drift correction and
    // hope the gyros are OK for a while. Just slowly reduce _omega_P
    // to prevent previous bad accels from throwing us off
    if (!_ins.healthy()) {
        error.zero();
    } else {
        // convert the error term to body frame
        error = _dcm_matrix.mul_transpose(error);
    }

    if (error.is_nan() || error.is_inf()) {
        // don't allow bad values
        check_matrix();
        return;
    }

    _error_rp_sum += error.length();
    _error_rp_count++;

    // base the P gain on the spin rate
    float spin_rate = _omega.length();

    // we now want to calculate _omega_P and _omega_I. The
    // _omega_P value is what drags us quickly to the
    // accelerometer reading.
    _omega_P = error * _P_gain(spin_rate) * _kp;
    if (_flags.fast_ground_gains) {
        _omega_P *= 8;
    }

    if (_flags.fly_forward && _gps && _gps->status() >= GPS::GPS_OK_FIX_2D &&
            _gps->ground_speed_cm < GPS_SPEED_MIN &&
            _ins.get_accel().x >= 7 &&
            pitch_sensor > -3000 && pitch_sensor < 3000) {
        // assume we are in a launch acceleration, and reduce the
        // rp gain by 50% to reduce the impact of GPS lag on
        // takeoff attitude when using a catapult
        _omega_P *= 0.5f;
    }

    // accumulate some integrator error
    if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        _omega_I_sum += error * _ki * _ra_deltat;
        _omega_I_sum_time += _ra_deltat;
    }

    if (_omega_I_sum_time >= 5) {
        // limit the rate of change of omega_I to the hardware
        // reported maximum gyro drift rate. This ensures that
        // short term errors don't cause a buildup of omega_I
        // beyond the physical limits of the device
        float change_limit = _gyro_drift_limit * _omega_I_sum_time;
        _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
        _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
        _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
        _omega_I += _omega_I_sum;
        _omega_I_sum.zero();
        _omega_I_sum_time = 0;
    }

    // zero our accumulator ready for the next GPS step
    _ra_sum.zero();
    _ra_deltat = 0;
    _ra_sum_start = last_correction_time;

    // remember the velocity for next time
    _last_velocity = velocity;

    if (_have_gps_lock && _flags.fly_forward) {
        // update wind estimate
        estimate_wind(velocity);
    }
}
Ejemplo n.º 4
0
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Cross( const Vector3f& A, const Vector3f& B )
{    
    return A.Cross( B );
}
Ejemplo n.º 5
0
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Normalize( const Vector3f& A )
{
    Vector3f vec = A;
    vec.Normalize();
    return vec;
}
Ejemplo n.º 6
0
bool AP_Arming::compass_checks(bool report)
{
    if ((checks_to_perform) & ARMING_CHECK_ALL ||
        (checks_to_perform) & ARMING_CHECK_COMPASS) {

        if (!_compass.use_for_yaw()) {
            // compass use is disabled
            return true;
        }

        if (!_compass.healthy()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass not healthy"));
            }
            return false;
        }
        // check compass learning is on or offsets have been set
        if (!_compass.learn_offsets_enabled() && !_compass.configured()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass not calibrated"));
            }
            return false;
        }

        //check if compass is calibrating
        if (_compass.is_calibrating()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("Arm: Compass calibration running"));
            }
            return false;
        }

        //check if compass has calibrated and requires reboot
        if (_compass.compass_cal_requires_reboot()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass calibrated requires reboot"));
            }
            return false;
        }

        // check for unreasonable compass offsets
        Vector3f offsets = _compass.get_offsets();
        if (offsets.length() > AP_ARMING_COMPASS_OFFSETS_MAX) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Compass offsets too high"));
            }
            return false;
        }

        // check for unreasonable mag field length
        float mag_field = _compass.get_field().length();
        if (mag_field > AP_ARMING_COMPASS_MAGFIELD_MAX || mag_field < AP_ARMING_COMPASS_MAGFIELD_MIN) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL, PSTR("PreArm: Check mag field"));
            }
            return false;
        }

        // check all compasses point in roughly same direction
        if (!_compass.consistent()) {
            if (report) {
                GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_CRITICAL,PSTR("PreArm: inconsistent compasses"));
    }
            return false;
        }
    }

    return true;
}
void MotionCombinator::update(JointRequest& jointRequest)
{
  specialActionOdometry += theSpecialActionsOutput.odometryOffset;

  const JointRequest* jointRequests[MotionRequest::numOfMotions];
  jointRequests[MotionRequest::walk] = &theWalkingEngineOutput;
  jointRequests[MotionRequest::kick] = &theKickEngineOutput;
  jointRequests[MotionRequest::specialAction] = &theSpecialActionsOutput;
  jointRequests[MotionRequest::stand] = &theStandOutput;
  jointRequests[MotionRequest::getUp] = &theGetUpEngineOutput;
  jointRequests[MotionRequest::dmpKick] = &theDmpKickEngineOutput;

  const JointRequest* armJointRequests[ArmMotionRequest::numOfArmMotions];
  armJointRequests[ArmMotionRequest::none] = &theNonArmeMotionEngineOutput;
  armJointRequests[ArmMotionRequest::keyFrame] = &theArmKeyFrameEngineOutput;

  jointRequest.angles[Joints::headYaw] = theHeadJointRequest.pan;
  jointRequest.angles[Joints::headPitch] = theHeadJointRequest.tilt;

  copy(*jointRequests[theMotionSelection.targetMotion], jointRequest);

  ASSERT(jointRequest.isValid());

  // Find fully active motion and set MotionInfo
  if(theMotionSelection.ratios[theMotionSelection.targetMotion] == 1.f)
  {
    Pose2f odometryOffset;
    // default values
    motionInfo.motion = theMotionSelection.targetMotion;
    motionInfo.isMotionStable = true;
    motionInfo.upcomingOdometryOffset = Pose2f();

    lastJointAngles = theJointAngles;

    switch(theMotionSelection.targetMotion)
    {
      case MotionRequest::walk:
        odometryOffset = theWalkingEngineOutput.odometryOffset;
        motionInfo.walkRequest = theWalkingEngineOutput.executedWalk;
        motionInfo.upcomingOdometryOffset = theWalkingEngineOutput.upcomingOdometryOffset;
        break;
      case MotionRequest::kick:
        odometryOffset = theKickEngineOutput.odometryOffset;
        motionInfo.kickRequest = theKickEngineOutput.executedKickRequest;
        motionInfo.isMotionStable = theKickEngineOutput.isStable;
        break;
      case MotionRequest::specialAction:
        odometryOffset = specialActionOdometry;
        specialActionOdometry = Pose2f();
        motionInfo.specialActionRequest = theSpecialActionsOutput.executedSpecialAction;
        motionInfo.isMotionStable = theSpecialActionsOutput.isMotionStable;
        break;
      case MotionRequest::getUp:
        motionInfo.isMotionStable = false;
        odometryOffset = theGetUpEngineOutput.odometryOffset;
        break;
      case MotionRequest::stand:
      case MotionRequest::dmpKick:
      default:
        break;
    }

    if(theRobotInfo.hasFeature(RobotInfo::zGyro) && (theFallDownState.state == FallDownState::upright || theMotionSelection.targetMotion == MotionRequest::getUp))
    {
      Vector3f rotatedGyros = theInertialData.orientation * theInertialData.gyro.cast<float>();
      odometryOffset.rotation = rotatedGyros.z() * theFrameInfo.cycleTime;
    }

    odometryData += odometryOffset;
    ASSERT(jointRequest.isValid());
  }
  else // interpolate motions
  {
    const bool interpolateStiffness = !(theMotionSelection.targetMotion != MotionRequest::specialAction && theMotionSelection.specialActionRequest.specialAction == SpecialActionRequest::playDead &&
                                       theMotionSelection.ratios[MotionRequest::specialAction] > 0.f); // do not interpolate from play_dead
    for(int i = 0; i < MotionRequest::numOfMotions; ++i)
      if(i != theMotionSelection.targetMotion && theMotionSelection.ratios[i] > 0.)
      {
        interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::headYaw, Joints::headPitch);
        if(theArmMotionSelection.armRatios[ArmMotionRequest::none] == 1)
          interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::lShoulderPitch, Joints::lHand);
        if(theArmMotionSelection.armRatios[theArmMotionSelection.rightArmRatiosOffset + ArmMotionRequest::none] == 1)
          interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::rShoulderPitch, Joints::rHand);
        interpolate(*jointRequests[i], *jointRequests[theMotionSelection.targetMotion], theMotionSelection.ratios[i], jointRequest, interpolateStiffness, Joints::lHipYawPitch, Joints::rAnkleRoll);
      }
  }

  ASSERT(jointRequest.isValid());

  auto combinateArmMotions = [&](Arms::Arm const arm)
  {
    const int ratioIndexOffset = arm * theArmMotionSelection.rightArmRatiosOffset;
    const Joints::Joint startJoint = arm == Arms::left ? Joints::lShoulderPitch : Joints::rShoulderPitch;
    const Joints::Joint endJoint = arm == Arms::left ? Joints::lHand : Joints::rHand;

    if(theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] != 1.f)
    {
      if(theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] > 0 &&
         ArmMotionRequest::none != theArmMotionSelection.targetArmMotion[arm])
        copy(jointRequest, theNonArmeMotionEngineOutput, startJoint, endJoint);

      if(ArmMotionRequest::none != theArmMotionSelection.targetArmMotion[arm])
        copy(*armJointRequests[theArmMotionSelection.targetArmMotion[arm]], jointRequest, startJoint, endJoint);
    }

    if(theArmMotionSelection.armRatios[ratioIndexOffset + theArmMotionSelection.targetArmMotion[arm]] == 1.f)
    {
      armMotionInfo.armMotion[arm] = theArmMotionSelection.targetArmMotion[arm];

      switch(theArmMotionSelection.targetArmMotion[arm])
      {
        case ArmMotionRequest::keyFrame:
          armMotionInfo.armKeyFrameRequest = theArmMotionSelection.armKeyFrameRequest;
          break;
        case ArmMotionRequest::none:
        default:
          break;
      }
    }
    else
    {
      const bool interpolateStiffness = !(theMotionSelection.targetMotion != MotionRequest::specialAction &&
                                         theMotionSelection.specialActionRequest.specialAction == SpecialActionRequest::playDead &&
                                         theMotionSelection.ratios[MotionRequest::specialAction] > 0.f &&
                                         theArmMotionSelection.armRatios[ratioIndexOffset + ArmMotionRequest::none] > 0);

      const JointRequest toJointRequest = theArmMotionSelection.targetArmMotion[arm] == ArmMotionRequest::none ?
                                          *jointRequests[theMotionSelection.targetMotion] : *armJointRequests[theArmMotionSelection.targetArmMotion[arm]];

      for(int i = 0; i < ArmMotionRequest::numOfArmMotions; ++i)
      {
        if(i != theArmMotionSelection.targetArmMotion[arm] && theArmMotionSelection.armRatios[ratioIndexOffset + i] > 0)
        {
          interpolate(*armJointRequests[i], toJointRequest, theArmMotionSelection.armRatios[ratioIndexOffset + i], jointRequest, interpolateStiffness, startJoint, endJoint);
        }
      }
    }

    ASSERT(jointRequest.isValid());
  };

  combinateArmMotions(Arms::left);
  combinateArmMotions(Arms::right);

  if(emergencyOffEnabled)
  {
    if(theFallDownState.state == FallDownState::falling && motionInfo.motion != MotionRequest::specialAction)
    {
      saveFall(jointRequest);
      centerHead(jointRequest);
      centerArms(jointRequest);
      currentRecoveryTime = 0;

      ASSERT(jointRequest.isValid());
    }
    else if((theFallDownState.state == FallDownState::staggering || theFallDownState.state == FallDownState::onGround) && (motionInfo.motion != MotionRequest::specialAction))
    {
      centerHead(jointRequest);

      ASSERT(jointRequest.isValid());
    }
    else
    {
      if(theFallDownState.state == FallDownState::upright)
      {
        headJawInSavePosition = false;
        headPitchInSavePosition = false;
        isFallingStarted = false;
      }

      if(currentRecoveryTime < recoveryTime)
      {
        currentRecoveryTime += 1;
        float ratio = (1.f / float(recoveryTime)) * currentRecoveryTime;
        for(int i = 0; i < Joints::numOfJoints; i++)
        {
          jointRequest.stiffnessData.stiffnesses[i] = 30 + int(ratio * float(jointRequest.stiffnessData.stiffnesses[i] - 30));
        }
      }
    }
  }

  float sum(0);
  int count(0);
  for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
  {
    if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore && lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
      count++;
    }
  }
  PLOT("module:MotionCombinator:deviations:JointRequest:legsOnly", sum / count);
  for(int i = 0; i < Joints::lHipYawPitch; i++)
  {
    if(jointRequest.angles[i] != JointAngles::off && jointRequest.angles[i] != JointAngles::ignore && lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += abs(jointRequest.angles[i] - lastJointRequest.angles[i]);
      count++;
    }
  }
  PLOT("module:MotionCombinator:deviations:JointRequest:all", sum / count);

  sum = 0;
  count = 0;
  for(int i = Joints::lHipYawPitch; i < Joints::numOfJoints; i++)
  {
    if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
      count++;
    }
  }
  PLOT("module:MotionCombinator:differenceToJointData:legsOnly", sum / count);

  for(int i = 0; i < Joints::lHipYawPitch; i++)
  {
    if(lastJointRequest.angles[i] != JointAngles::off && lastJointRequest.angles[i] != JointAngles::ignore)
    {
      sum += abs(lastJointRequest.angles[i] - theJointAngles.angles[i]);
      count++;
    }
  }
  lastJointRequest = jointRequest;
  PLOT("module:MotionCombinator:differenceToJointData:all", sum / count);

#ifndef NDEBUG
  if(!jointRequest.isValid())
  {
    {
      std::string logDir = "";
#ifdef TARGET_ROBOT
      logDir = "../logs/";
#endif
      OutMapFile stream(logDir + "jointRequest.log");
      stream << jointRequest;
      OutMapFile stream2(logDir + "motionSelection.log");
      stream2 << theMotionSelection;
    }
    ASSERT(false);
  }
#endif
}
Ejemplo n.º 8
0
void SCollision::performCollision(){
	float playerHitRadius = 0.2;
	if(!sound){ sound = new Sound("sounds/clang.wav"); }

	GameRoom *gr = SCollision::gameState->GetRoom();
	//gr->monitor.Enter('r'); //What the hell's this for?
	vector<GameObject*> obs = gr->GetGameObjects();
	Camera *cam  = SCollision::gameState->GetCamera();

	list<Projectile *> *bullets = SCollision::gameState->GetParticleSystems()->GetBullets();  	
	list<Projectile *>::iterator it1 = bullets->begin(); 
	while(it1 != bullets->end()){  
		Projectile *p1 = *it1;
		if(p1->isDead()){it1++; continue;}
		if(p1->typeString == "Enemy"){
			//calculate collision between enemy and bullets
			list<Projectile *>::iterator it2 = bullets->begin();
			while(it2 != bullets->end()){ 
				Projectile *p2 = *it2;
				if(p2->isDead()){it2++; continue;}
				if(p2->owner == "player" && p2->typeString != "NavShot"){ //enemy should take damage at collision
					if( (p1->getPosition() - p2->getPosition()).norm() < (p1->hitRadius+p2->hitRadius)){
						//enemy p1 takes damage -> use hit() to code them
						sound->Play();
						p1->health = p1->health - p2->damage;
						p2->hit(p2->getPosition());
						p1->hit(p1->getPosition());
						if(p1->isDead()){
							//create and attach explosion where enemy was
							Projectile *pNew = new SmokyBullet(p1->getPosition(),
								Vector3f(0.0,0.0,0.0), 0.8, 0.1, 0.1, 0.7);
							pNew->hit(p1->getPosition());
							gameState->AddProjectile(pNew);
						}
					}
				}
				it2++;
			}
		}
		//calculate collision between player and all other projectiles
		if(p1->owner != "player" && p1->typeString != "NavShot"){
			if( (cam->getPosition() - p1->getPosition()).norm() < (playerHitRadius+p1->hitRadius)){
				//player takes damage
				sound->Play();
				Render::health = Render::health - p1->damage;
				Render::hitEffect();
				p1->hit(p1->getPosition());
			}
		}


		//process firing bullets
		if(p1->firesOwnBullets && p1->fireBulletCountDown <= 0.0){  //fire a bullet towards cam
			p1->fireBulletCountDown = 20000.0 * (rand()%100000 / 100000.0) ;;
			Vector3f shootDir = (cam->getPosition() - p1->getPosition());
			Vector3f delta =  Vector3f( ( (rand()%100000+1) / 100000.0)-0.5 , 
										( (rand()%100000+1) / 100000.0)-0.5 , 
										( (rand()%100000+1) / 100000.0)-0.5  );
			shootDir = shootDir + delta*0.3;
			shootDir = shootDir.normalized()*2;
			Projectile *pNew = new Slug(p1->getPosition(),
				shootDir, 0.8, 0.1, 0.1, 0.7);
			pNew->owner = "enemy";
			pNew->damage = 5.0;
			//pNew->hit(p1->getPosition());
			gameState->AddProjectile(pNew);
		}

		it1++; 
	}

	
	//deleteDeadEnemies from repel list
	ProjectileEnemies::deleteDead();


	//gr->monitor.Exit('r'); //seriously, what's this for?
}
Ejemplo n.º 9
0
Vector3f Sphere::getNormal(Vector3f point){
    Vector3f N = point - center;
    return N / N.getLength();
}
Ejemplo n.º 10
0
Forest::Forest(Actor* parent, ForestDesc* desc) : Actor( parent )
{
    assert( desc );
    assert( desc->surface );
    assert( desc->assetName.length() );
    assert( desc->cache.length() );

    // copy descriptor
    _desc = *desc;    

    // load asset
    _asset = Gameplay::iEngine->createAsset( engine::atBinary, _desc.assetName.c_str() );

    // enumerate clumps
    callback::ClumpL clumps;    
    _asset->forAllClumps( callback::enumerateClumps, &clumps );
    assert( clumps.size() );

    // fill batch schemes
    Matrix4f clumpM;
    MatrixConversion trunkConversion;
    MatrixConversion canopyConversion;
    _canopyScheme.numLods = _trunkScheme.numLods = clumps.size();
    for( callback::ClumpI clumpI = clumps.begin(); clumpI != clumps.end(); clumpI++ )
    {
        // determine lod Id and check for consistency
        unsigned int lodId = getClumpLodId( *clumpI );
        if( _trunkScheme.lodGeometry[lodId] )
        {
            throw Exception( "Clump \"%s\" is a duplication of existing LOD!", (*clumpI)->getName() );
        }
        // fill schemes
        engine::IAtomic* trunkAtomic     = Gameplay::iEngine->getAtomic( *clumpI, Gameplay::iEngine->findFrame( (*clumpI)->getFrame(), "Trunk" ) ); assert( trunkAtomic );
        engine::IAtomic* canopyAtomic    = Gameplay::iEngine->getAtomic( *clumpI, Gameplay::iEngine->findFrame( (*clumpI)->getFrame(), "Canopy" ) ); assert( canopyAtomic );
        _trunkScheme.lodGeometry[lodId]  = trunkAtomic->getGeometry();        
        _canopyScheme.lodGeometry[lodId] = canopyAtomic->getGeometry();
        _trunkScheme.lodDistance[lodId] = _canopyScheme.lodDistance[lodId] = _desc.lodDistance[lodId];
        // calculate conversions for nearest LOD
        if( lodId == 0 )
        {
            clumpM = (*clumpI)->getFrame()->getLTM();
            trunkConversion.setup( clumpM, trunkAtomic->getFrame()->getLTM() );
            canopyConversion.setup( clumpM, canopyAtomic->getFrame()->getLTM() );
        }
    }

    _trunkScheme.flags = _canopyScheme.flags = 0;

    // check schemes
    assert( _trunkScheme.isValid() );
    assert( _canopyScheme.isValid() );

    // create full cache names
    std::string instanceCache  = _desc.cache; instanceCache  += ".matrices";
    std::string trunkBspCache  = _desc.cache; trunkBspCache  += ".trunk";
    std::string canopyBspCache = _desc.cache; canopyBspCache += ".canopy";

    // try to load forest from cache
    IResource* resource = getCore()->getResource( instanceCache.c_str(), "rb" );
    if( resource )
    {
        unsigned int numTrees;
        fread( &numTrees, sizeof(unsigned int), 1, resource->getFile() );
        _treeMatrix.resize( numTrees );
        fread( &_treeMatrix[0], sizeof(Matrix4f), numTrees, resource->getFile() );
        resource->release();
    }
    else
    {
        // obtain surface properties
        Matrix4f ltm = _desc.surface->getFrame()->getLTM();
        engine::IGeometry* geometry = _desc.surface->getGeometry();
        engine::Mesh* mesh = geometry->createMesh();

        float preservedDistance = _desc.collScale * ( geometry->getAABBSup() - geometry->getAABBInf() ).length();

        // iterate surface triangles
        Vector3f vertex[3];
        Vector3f edge[3];
        Vector3f edgeN[2];
        Vector3f normal;
        Vector3f pos;
        float cosA, sinA, angle, square, probability, scale;
        unsigned int i,j,numTreesInTriangle;
        Matrix4f instanceM;    
        for( i=0; i<mesh->numTriangles; i++ )
        {
            // transform triangle vertices to world space
            vertex[0] = Gameplay::iEngine->transformCoord( mesh->vertices[mesh->triangles[i].vertexId[0]], ltm );
            vertex[1] = Gameplay::iEngine->transformCoord( mesh->vertices[mesh->triangles[i].vertexId[1]], ltm );
            vertex[2] = Gameplay::iEngine->transformCoord( mesh->vertices[mesh->triangles[i].vertexId[2]], ltm );
            // calculate triangle square value...
            edge[0] = vertex[1] - vertex[0];
            edge[1] = vertex[2] - vertex[0];
            edge[2] = vertex[2] - vertex[1];
            edgeN[0] = edge[0]; edgeN[0].normalize();
            edgeN[1] = edge[1]; edgeN[1].normalize();
            normal.cross( edgeN[0], edgeN[1] );
            cosA = Vector3f::dot( edgeN[0], edgeN[1] );
            if( cosA > 1.0f ) angle = 0.0f; 
            else if( cosA < -1.0f ) angle = 180; 
            else angle = acosf( cosA );
            sinA = sin( angle );
            square = 0.5f * edge[0].length() * edge[1].length() * sinA / 10000.0f;
            // obtain number of particles in this triangle
            numTreesInTriangle = unsigned int( square * _desc.density );
            if( !numTreesInTriangle )
            {
                // include probability method to decide to place grass on to this triangle
                probability = square / ( 1 / _desc.density );
                assert( probability <= 1.0f );
                if( probability > 0 && getCore()->getRandToolkit()->getUniform( 0, 1 ) <= probability ) numTreesInTriangle++;
            }
            // generate trees
            for( j=0; j<numTreesInTriangle; j++ )
            {
                // generate scale
                scale = getCore()->getRandToolkit()->getUniform( _desc.minScale, _desc.maxScale );
                // generate coordinate
                pos = generateRandomPosition( vertex, edge );                
                // generate matrix
                instanceM.set(
                    clumpM[0][0] * scale, clumpM[0][1] * scale, clumpM[0][2] * scale, 0.0f,
                    clumpM[1][0] * scale, clumpM[1][1] * scale, clumpM[1][2] * scale, 0.0f,
                    clumpM[2][0] * scale, clumpM[2][1] * scale, clumpM[2][2] * scale, 0.0f,
                    0.0f, 0.0f, 0.0f, 1.0f
                );
                instanceM = Gameplay::iEngine->rotateMatrix( instanceM, Vector3f(0,1,0), getCore()->getRandToolkit()->getUniform(0,360) );
                instanceM[3][0] = pos[0];
                instanceM[3][1] = pos[1];
                instanceM[3][2] = pos[2];
                _treeMatrix.push_back( instanceM );
            }
            Scene::progressCallback( 
                Gameplay::iLanguage->getUnicodeString(844),
                float( i ) / float( mesh->numTriangles ),
                getScene()
            );
        }

        // release temporary resources
        Gameplay::iEngine->releaseMesh( mesh );

        // write solution
        ccor::IResource* resource = getCore()->getResource( instanceCache.c_str(), "wb" );
        unsigned int numTrees = _treeMatrix.size();
        fwrite( &numTrees, sizeof(unsigned int), 1, resource->getFile() );
        fwrite( &_treeMatrix[0], sizeof(Matrix4f), numTrees, resource->getFile() );
        resource->release();
    }
    
    // build batches
    _trunkBatch  = Gameplay::iEngine->createBatch( _treeMatrix.size(), &_trunkScheme ); assert( _trunkBatch );
    _canopyBatch = Gameplay::iEngine->createBatch( _treeMatrix.size(), &_canopyScheme ); assert( _canopyBatch );
    // fill batches
    for( unsigned int i=0; i<_treeMatrix.size(); i++ )
    {
        _trunkBatch->setMatrix( i, trunkConversion.convert( _treeMatrix[i] ) );
        _canopyBatch->setMatrix( i, canopyConversion.convert( _treeMatrix[i] ) );
    }

    // create spatial acceleration structures BSP trees
    _trunkBatch->createBatchTree( _desc.bspLeafSize, trunkBspCache.c_str() );
    _canopyBatch->createBatchTree( _desc.bspLeafSize, canopyBspCache.c_str() );
    
    // add batches to scene
    getScene()->getStage()->add( _trunkBatch );
    getScene()->getStage()->add( _canopyBatch );

    // add layers to scene
    if( _desc.layers )
    {
        getScene()->getStage()->add( _desc.layers );
    }

    // set no sound
    _rustleSound = _squeakSound = NULL;
}
Ejemplo n.º 11
0
static float getSquaredLength(const Vector3f& v)
{
    return v.dot( v );
}
Ejemplo n.º 12
0
int main() {

    // read in the file
    Coords posvel("dm_1.0000.bin");
    cout << "Read in " << posvel.npart << " particles\n";

    // Exercise the interface and code
    {
           Vector3f av;
           Map< MatrixXf > epos = posvel.pos.matrix();
           av = epos.rowwise().sum()/ posvel.npart;
           cout << "Average position : " << av.transpose() << endl;

           Map< MatrixXf > evel = posvel.vel.matrix();
           av = evel.rowwise().sum()/ posvel.npart;
           cout << "Average velocity : " << av.transpose() << endl;
    }


    // Define the grid
    MA<array4d> grid(extents[4][Ngrid][Ngrid][Ngrid+2]); // Pad for FFT, 1 component for overdensity, 3 for velocity
    // Define views on this grid
    MA<view4_4d> rgrid = grid.view<view4_4d>(indices[r_()][r_()][r_()][r_(0, Ngrid)]);
    MA<ref4c> cgrid ( new ref4c(reinterpret_cast< complex<double>* >(grid.ref()), extents[4][Ngrid][Ngrid][Ngrid/2 + 1]));
    MA<array4d::reference> dense = rgrid.sub(0);


    // CIC grid
    cout << "Integrated density, density-weighted velocity -->\n";
    for (int ii = 0; ii < 4; ++ii) {
        cic(rgrid.sub(ii), posvel, ii-1);
        cout << boost::format("%1$6i %2$20.10f\n") % ii % (rgrid.sub(ii).sum());
    }



    // Normalize by density
    int nzeros;
    cout << "Integrated velocity field -->\n";
    for (int ii=1; ii < 4; ++ii) {
        nzeros = 0;
        boost::function < void(double&, double&) > ff = if_(_2 > 0)[_1 /= _2].else_[var(nzeros)++];
        multi_for(rgrid.sub(ii), dense,ff);
        cout << boost::format("%1$6i %2$20.10f %3$10i\n") % ii % (rgrid.sub(ii).sum()*Lbox) % nzeros;
       }
    // Normalize the density by rho_mean
    cout << dense.sum() << endl;
    dense /= static_cast<double>(posvel.npart)/pow( static_cast<double>(Ngrid), 3);
    cout << dense.sum() << endl;


    // FFT
    fftw_forward(grid);


    // Compute k_i v_i for each grid separately
    kdot_impl kdot;
    for (int ii=1; ii < 4; ++ii) {
        kdot.idim = ii-1;
        multi_for_native_indices(cgrid.sub(ii), kdot);
    }

    // Now add the vectors together --- things are contiguous, so go ahead and use the
    // easy route
    cgrid.sub(1)() += cgrid.sub(2)() + cgrid.sub(3)();
    // This should be what we need modulo normalization factors

    // Compute P(k)
    PkStruct pk;
    double lkmin = log(0.008); double lkmax = log(0.3);
    pk.setbins(lkmin, lkmax, 20);

    // We want to compute delta* delta, delta* theta, and theta* theta
    complex_mult_impl cmult;
    // First do delta* theta -- and store this in cgrid.sub(2)
    multi_for(cgrid.sub(0), cgrid.sub(1), cgrid.sub(2), cmult);
    // delta* delta and theta* theta -- store in place
    multi_for(cgrid.sub(0), cgrid.sub(0), cgrid.sub(0), cmult); // We do it this way to make the connection clear with delta* theta
    multi_for(cgrid.sub(1), cgrid.sub(1), cgrid.sub(1), cmult); // We do it this way to make the connection clear with delta* theta


    // Do the matter power spectrum
    multi_for_native_indices(cgrid.sub(0), pk);
    cout << pk.pk() << endl;
    writeMatrix("pk_matter.dat", "%1$10.4e ", pk.pk());

    // Do the velocity power spectrum
    pk.reset();
    multi_for_native_indices(cgrid.sub(1), pk);
    double fac; fac = pow(Lbox, 2);
    cout << pk.pk(fac) << endl; // The true adds in the correct velocity normalization
    writeMatrix("pk_theta.dat", "%1$10.4e ", pk.pk(fac));


    // Do the cross power spectrum
    pk.reset();
    multi_for_native_indices(cgrid.sub(2), pk);
    fac = Lbox;
    cout << pk.pk(fac) << endl; // The true adds in the correct velocity normalization
    writeMatrix("pk_delta_theta.dat", "%1$10.4e ", pk.pk(fac));

}
Ejemplo n.º 13
0
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This drift correction implementation is based on a paper
// by Bill Premerlani from here:
//   http://gentlenav.googlecode.com/files/RollPitchDriftCompensation.pdf
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    Vector3f velocity;
    uint32_t last_correction_time;

    // perform yaw drift correction if we have a new yaw reference
    // vector
    drift_correction_yaw();

    // rotate accelerometer values into the earth frame
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (_ins.get_accel_health(i)) {
            /*
              by using get_delta_velocity() instead of get_accel() the
              accel value is sampled over the right time delta for
              each sensor, which prevents an aliasing effect
             */
            Vector3f delta_velocity;
            float delta_velocity_dt;
            _ins.get_delta_velocity(i, delta_velocity);
            delta_velocity_dt = _ins.get_delta_velocity_dt(i);
            if (delta_velocity_dt > 0) {
                _accel_ef[i] = _dcm_matrix * (delta_velocity / delta_velocity_dt);
                // integrate the accel vector in the earth frame between GPS readings
                _ra_sum[i] += _accel_ef[i] * deltat;
            }
        }
    }

    //update _accel_ef_blended
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_75
    if (_ins.get_accel_count() == 2 && _ins.use_accel(0) && _ins.use_accel(1)) {
        float imu1_weight_target = _active_accel_instance == 0 ? 1.0f : 0.0f;
        // slew _imu1_weight over one second
        _imu1_weight += constrain_float(imu1_weight_target-_imu1_weight, -deltat, deltat);
        _accel_ef_blended = _accel_ef[0] * _imu1_weight + _accel_ef[1] * (1.0f - _imu1_weight);
    } else {
        _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
    }
#else
    _accel_ef_blended = _accel_ef[_ins.get_primary_accel()];
#endif // HAL_CPU_CLASS >= HAL_CPU_CLASS_75

    // keep a sum of the deltat values, so we know how much time
    // we have integrated over
    _ra_deltat += deltat;

    if (!have_gps() ||
            _gps.status() < AP_GPS::GPS_OK_FIX_3D ||
            _gps.num_sats() < _gps_minsats) {
        // no GPS, or not a good lock. From experience we need at
        // least 6 satellites to get a really reliable velocity number
        // from the GPS.
        //
        // As a fallback we use the fixed wing acceleration correction
        // if we have an airspeed estimate (which we only have if
        // _fly_forward is set), otherwise no correction
        if (_ra_deltat < 0.2f) {
            // not enough time has accumulated
            return;
        }
        float airspeed;
        if (airspeed_sensor_enabled()) {
            airspeed = _airspeed->get_airspeed();
        } else {
            airspeed = _last_airspeed;
        }
        // use airspeed to estimate our ground velocity in
        // earth frame by subtracting the wind
        velocity = _dcm_matrix.colx() * airspeed;

        // add in wind estimate
        velocity += _wind;

        last_correction_time = hal.scheduler->millis();
        _have_gps_lock = false;
    } else {
        if (_gps.last_fix_time_ms() == _ra_sum_start) {
            // we don't have a new GPS fix - nothing more to do
            return;
        }
        velocity = _gps.velocity();
        last_correction_time = _gps.last_fix_time_ms();
        if (_have_gps_lock == false) {
            // if we didn't have GPS lock in the last drift
            // correction interval then set the velocities equal
            _last_velocity = velocity;
        }
        _have_gps_lock = true;

        // keep last airspeed estimate for dead-reckoning purposes
        Vector3f airspeed = velocity - _wind;
        airspeed.z = 0;
        _last_airspeed = airspeed.length();
    }

    if (have_gps()) {
        // use GPS for positioning with any fix, even a 2D fix
        _last_lat = _gps.location().lat;
        _last_lng = _gps.location().lng;
        _position_offset_north = 0;
        _position_offset_east = 0;

        // once we have a single GPS lock, we can update using
        // dead-reckoning from then on
        _have_position = true;
    } else {
        // update dead-reckoning position estimate
        _position_offset_north += velocity.x * _ra_deltat;
        _position_offset_east  += velocity.y * _ra_deltat;
    }

    // see if this is our first time through - in which case we
    // just setup the start times and return
    if (_ra_sum_start == 0) {
        _ra_sum_start = last_correction_time;
        _last_velocity = velocity;
        return;
    }

    // equation 9: get the corrected acceleration vector in earth frame. Units
    // are m/s/s
    Vector3f GA_e;
    GA_e = Vector3f(0, 0, -1.0f);

    if (_ra_deltat <= 0) {
        // waiting for more data
        return;
    }
    
    bool using_gps_corrections = false;
    float ra_scale = 1.0f/(_ra_deltat*GRAVITY_MSS);

    if (_flags.correct_centrifugal && (_have_gps_lock || _flags.fly_forward)) {
        float v_scale = gps_gain.get() * ra_scale;
        Vector3f vdelta = (velocity - _last_velocity) * v_scale;
        GA_e += vdelta;
        GA_e.normalize();
        if (GA_e.is_inf()) {
            // wait for some non-zero acceleration information
            _last_failure_ms = hal.scheduler->millis();
            return;
        }
        using_gps_corrections = true;
    }

    // calculate the error term in earth frame.
    // we do this for each available accelerometer then pick the
    // accelerometer that leads to the smallest error term. This takes
    // advantage of the different sample rates on different
    // accelerometers to dramatically reduce the impact of aliasing
    // due to harmonics of vibrations that match closely the sampling
    // rate of our accelerometers. On the Pixhawk we have the LSM303D
    // running at 800Hz and the MPU6000 running at 1kHz, by combining
    // the two the effects of aliasing are greatly reduced.
    Vector3f error[INS_MAX_INSTANCES];
    float error_dirn[INS_MAX_INSTANCES];
    Vector3f GA_b[INS_MAX_INSTANCES];
    int8_t besti = -1;
    float best_error = 0;
    for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
        if (!_ins.get_accel_health(i)) {
            // only use healthy sensors
            continue;
        }
        _ra_sum[i] *= ra_scale;

        // get the delayed ra_sum to match the GPS lag
        if (using_gps_corrections) {
            GA_b[i] = ra_delayed(i, _ra_sum[i]);
        } else {
            GA_b[i] = _ra_sum[i];
        }
        if (GA_b[i].is_zero()) {
            // wait for some non-zero acceleration information
            continue;
        }
        GA_b[i].normalize();
        if (GA_b[i].is_inf()) {
            // wait for some non-zero acceleration information
            continue;
        }
        error[i] = GA_b[i] % GA_e;
        // Take dot product to catch case vectors are opposite sign and parallel
        error_dirn[i] = GA_b[i] * GA_e;
        float error_length = error[i].length();
        if (besti == -1 || error_length < best_error) {
            besti = i;
            best_error = error_length;
        }
        // Catch case where orientation is 180 degrees out
        if (error_dirn[besti] < 0.0f) {
            best_error = 1.0f;
        }

    }

    if (besti == -1) {
        // no healthy accelerometers!
        _last_failure_ms = hal.scheduler->millis();
        return;
    }

    _active_accel_instance = besti;

#define YAW_INDEPENDENT_DRIFT_CORRECTION 0
#if YAW_INDEPENDENT_DRIFT_CORRECTION
    // step 2 calculate earth_error_Z
    float earth_error_Z = error.z;

    // equation 10
    float tilt = pythagorous2(GA_e.x, GA_e.y);

    // equation 11
    float theta = atan2f(GA_b[besti].y, GA_b[besti].x);

    // equation 12
    Vector3f GA_e2 = Vector3f(cosf(theta)*tilt, sinf(theta)*tilt, GA_e.z);

    // step 6
    error = GA_b[besti] % GA_e2;
    error.z = earth_error_Z;
#endif // YAW_INDEPENDENT_DRIFT_CORRECTION

    // to reduce the impact of two competing yaw controllers, we
    // reduce the impact of the gps/accelerometers on yaw when we are
    // flat, but still allow for yaw correction using the
    // accelerometers at high roll angles as long as we have a GPS
    if (AP_AHRS_DCM::use_compass()) {
        if (have_gps() && is_equal(gps_gain,1.0f)) {
            error[besti].z *= sinf(fabsf(roll));
        } else {
            error[besti].z = 0;
        }
    }

    // if ins is unhealthy then stop attitude drift correction and
    // hope the gyros are OK for a while. Just slowly reduce _omega_P
    // to prevent previous bad accels from throwing us off
    if (!_ins.healthy()) {
        error[besti].zero();
    } else {
        // convert the error term to body frame
        error[besti] = _dcm_matrix.mul_transpose(error[besti]);
    }

    if (error[besti].is_nan() || error[besti].is_inf()) {
        // don't allow bad values
        check_matrix();
        _last_failure_ms = hal.scheduler->millis();
        return;
    }

    _error_rp = 0.8f * _error_rp + 0.2f * best_error;

    // base the P gain on the spin rate
    float spin_rate = _omega.length();

    // sanity check _kp value
    if (_kp < AP_AHRS_RP_P_MIN) {
        _kp = AP_AHRS_RP_P_MIN;
    }

    // we now want to calculate _omega_P and _omega_I. The
    // _omega_P value is what drags us quickly to the
    // accelerometer reading.
    _omega_P = error[besti] * _P_gain(spin_rate) * _kp;
    if (use_fast_gains()) {
        _omega_P *= 8;
    }

    if (_flags.fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
            _gps.ground_speed() < GPS_SPEED_MIN &&
            _ins.get_accel().x >= 7 &&
            pitch_sensor > -3000 && pitch_sensor < 3000) {
        // assume we are in a launch acceleration, and reduce the
        // rp gain by 50% to reduce the impact of GPS lag on
        // takeoff attitude when using a catapult
        _omega_P *= 0.5f;
    }

    // accumulate some integrator error
    if (spin_rate < ToRad(SPIN_RATE_LIMIT)) {
        _omega_I_sum += error[besti] * _ki * _ra_deltat;
        _omega_I_sum_time += _ra_deltat;
    }

    if (_omega_I_sum_time >= 5) {
        // limit the rate of change of omega_I to the hardware
        // reported maximum gyro drift rate. This ensures that
        // short term errors don't cause a buildup of omega_I
        // beyond the physical limits of the device
        float change_limit = _gyro_drift_limit * _omega_I_sum_time;
        _omega_I_sum.x = constrain_float(_omega_I_sum.x, -change_limit, change_limit);
        _omega_I_sum.y = constrain_float(_omega_I_sum.y, -change_limit, change_limit);
        _omega_I_sum.z = constrain_float(_omega_I_sum.z, -change_limit, change_limit);
        _omega_I += _omega_I_sum;
        _omega_I_sum.zero();
        _omega_I_sum_time = 0;
    }

    // zero our accumulator ready for the next GPS step
    memset(&_ra_sum[0], 0, sizeof(_ra_sum));
    _ra_deltat = 0;
    _ra_sum_start = last_correction_time;

    // remember the velocity for next time
    _last_velocity = velocity;
}
Ejemplo n.º 14
0
void Mesh::_CalculateNormalsAndTangents()
{
	if (mT.IsValid() || mV.IsEmpty()) return;

	// Don't bother with points or lines
	if (mPrimitive == IGraphics::Primitive::Point	||
		mPrimitive == IGraphics::Primitive::Line	||
		mPrimitive == IGraphics::Primitive::LineStrip) return;

	// Allocate a temporary buffer to store binormals
	Array<Vector3f> binormals (mV.GetSize());

	// Remember whether we already have normals to work with
	bool calculateNormals  = (mV.GetSize() != mN.GetSize());

	// We can only calculate tangents if the texture coordinates are available
	bool calculateTangents = (mV.GetSize() == mTc0.GetSize());

	// If we should calculate normals, clear the existing normal array
	if (calculateNormals)
	{
		mN.Clear();
		mN.ExpandTo(mV.GetSize());
	}

	// Expand the tangent array
	if (calculateTangents) mT.ExpandTo(mV.GetSize());

	// The number of indices
	uint size = mIndices.IsValid() ? mIndices.GetSize() : GetNumberOfVertices();

	// Triangles
	if (size > 2)
	{
		bool even = true;
		uint i0, i1, i2;

		for (uint i = 0; i + 2 < size; )
		{
			i0 = i;
			i1 = i+1;
			i2 = i+2;

			if (mIndices.IsValid())
			{
				i0 = mIndices[i0];
				i1 = mIndices[i1];
				i2 = mIndices[i2];
			}

			ASSERT(i0 < mV.GetSize(), "Index out of bounds!");
			ASSERT(i1 < mV.GetSize(), "Index out of bounds!");
			ASSERT(i2 < mV.GetSize(), "Index out of bounds!");

			const Vector3f& v0 ( mV[i0] );
			const Vector3f& v1 ( mV[i1] );
			const Vector3f& v2 ( mV[i2] );

			Vector3f v10 (v1 - v0);
			Vector3f v20 (v2 - v0);

			if (calculateNormals)
			{
				Vector3f normal (Cross(v10, v20));

				mN[i0] += normal;
				mN[i1] += normal;
				mN[i2] += normal;
			}

			if (calculateTangents)
			{
				const Vector2f& t0 ( mTc0[i0] );
				const Vector2f& t1 ( mTc0[i1] );
				const Vector2f& t2 ( mTc0[i2] );

				Vector2f t10 (t1 - t0);
				Vector2f t20 (t2 - t0);

				float denominator = t10.x * t20.y - t20.x * t10.y;

				if ( Float::IsNotZero(denominator) )
				{
					float scale = 1.0f / denominator;

					Vector3f tangent ((v10.x * t20.y - v20.x * t10.y) * scale,
									  (v10.y * t20.y - v20.y * t10.y) * scale,
									  (v10.z * t20.y - v20.z * t10.y) * scale);

					Vector3f binormal((v20.x * t10.x - v10.x * t20.x) * scale,
									  (v20.y * t10.x - v10.y * t20.x) * scale,
									  (v20.z * t10.x - v10.z * t20.x) * scale);

					mT[i0] += tangent;
					mT[i1] += tangent;
					mT[i2] += tangent;

					binormals[i0] += binormal;
					binormals[i1] += binormal;
					binormals[i2] += binormal;
				}
			}

			if (mPrimitive == IGraphics::Primitive::Triangle)
			{
				i += 3;
			}
			else if (mPrimitive == IGraphics::Primitive::Quad)
			{
				if (even) ++i;
				else i += 3;
				even = !even;
			}
			else ++i;
		}

		// If we're calculating normals we need to match all normals with identical vertices
		if (calculateNormals)
		{
			Array<uint> matches;

			for (uint i = 0; i < mV.GetSize(); ++i)
			{
				matches.Clear();
				matches.Expand() = i;
				Vector3f N = mN[i];

				const Vector3f& V (mV[i]);

				for (uint b = 0; b < mV.GetSize(); ++b)
				{
					if (i != b && V == mV[b])
					{
						matches.Expand() = b;
						N += mN[b];
					}
				}

				N.Normalize();

				for (uint b = 0; b < matches.GetSize(); ++b)
				{
					mN[ matches[b] ] = N;
				}
			}
		}
	}

	if (calculateTangents)
	{
		// Normalize all tangents
		for (uint i = 0; i < mV.GetSize(); ++i)
		{
			Vector3f&  T (mT[i]);
			Vector3f&  B (binormals[i]);
			Vector3f&  N (mN[i]);

			// In order to avoid visible seams, the tangent should be 90 degrees to the normal
			// Note to self: Gram-Schmidt formula for the cross product below: T = T - N * Dot(N, T);
			T = Cross(B, N);
			T.Normalize();

			// Flip the tangent if the handedness is incorrect
			if (Dot(Cross(T, B), N) < 0.0f) T.Flip();
		}
	}
}
Ejemplo n.º 15
0
// _calibrate_model - perform low level accel calibration
// accel_sample are accelerometer samples collected in 6 different positions
// accel_offsets are output from the calibration routine
// accel_scale are output from the calibration routine
// returns true if successful
bool AP_InertialSensor::_calibrate_accel( Vector3f accel_sample[6], Vector3f& accel_offsets, Vector3f& accel_scale )
{
    int16_t i;
    int16_t num_iterations = 0;
    float eps = 0.000000001;
    float change = 100.0;
    float data[3];
    float beta[6];
    float delta[6];
    float ds[6];
    float JS[6][6];
    bool success = true;

    // reset
    beta[0] = beta[1] = beta[2] = 0;
    beta[3] = beta[4] = beta[5] = 1.0/GRAVITY;
    
    while( num_iterations < 20 && change > eps ) {
        num_iterations++;

        _calibrate_reset_matrices(ds, JS);

        for( i=0; i<6; i++ ) {
            data[0] = accel_sample[i].x;
            data[1] = accel_sample[i].y;
            data[2] = accel_sample[i].z;
            _calibrate_update_matrices(ds, JS, beta, data);
        }

        _calibrate_find_delta(ds, JS, delta);

        change =    delta[0]*delta[0] +
                    delta[0]*delta[0] +
                    delta[1]*delta[1] +
                    delta[2]*delta[2] +
                    delta[3]*delta[3] / (beta[3]*beta[3]) +
                    delta[4]*delta[4] / (beta[4]*beta[4]) +
                    delta[5]*delta[5] / (beta[5]*beta[5]);

        for( i=0; i<6; i++ ) {
            beta[i] -= delta[i];
        }
    }

    // copy results out
    accel_scale.x = beta[3] * GRAVITY;
    accel_scale.y = beta[4] * GRAVITY;
    accel_scale.z = beta[5] * GRAVITY;
    accel_offsets.x = beta[0] * accel_scale.x;
    accel_offsets.y = beta[1] * accel_scale.y;
    accel_offsets.z = beta[2] * accel_scale.z;

    // sanity check scale
    if( accel_scale.is_nan() || fabs(accel_scale.x-1.0) > 0.1 || fabs(accel_scale.y-1.0) > 0.1 || fabs(accel_scale.z-1.0) > 0.1 ) {
        success = false;
    }
    // sanity check offsets (2.0 is roughly 2/10th of a G, 5.0 is roughly half a G)
    if( accel_offsets.is_nan() || fabs(accel_offsets.x) > 2.0 || fabs(accel_offsets.y) > 2.0 || fabs(accel_offsets.z) > 2.0 ) {
        success = false;
    }

    // return success or failure
    return success;
}
Ejemplo n.º 16
0
//==============================
// VRMenuMgrLocal::SubmitForRenderingRecursive
void VRMenuMgrLocal::SubmitForRenderingRecursive( OvrDebugLines & debugLines, BitmapFont const & font, 
        BitmapFontSurface & fontSurface, VRMenuRenderFlags_t const & flags, VRMenuObjectLocal const * obj, 
        Posef const & parentModelPose, Vector4f const & parentColor, Vector3f const & parentScale, 
        Bounds3f & cullBounds, SubmittedMenuObject * submitted, int const maxIndices, int & curIndex ) const
{
	if ( curIndex >= maxIndices )
	{
		// If this happens we're probably not correctly clearing the submitted surfaces each frame
		// OR we've got a LOT of surfaces.
		LOG( "maxIndices = %i, curIndex = %i", maxIndices, curIndex );
		DROID_ASSERT( curIndex < maxIndices, "VrMenu" );
		return;
	}

	// check if this object is hidden
	if ( obj->GetFlags() & VRMENUOBJECT_DONT_RENDER )
	{
		return;
	}

	Posef const & localPose = obj->GetLocalPose();

	Posef curModelPose;
	curModelPose.Position = parentModelPose.Position + ( parentModelPose.Orientation * parentScale.EntrywiseMultiply( localPose.Position ) );
	curModelPose.Orientation = localPose.Orientation * parentModelPose.Orientation;

	Vector4f curColor = parentColor * obj->GetColor();
	Vector3f const & localScale = obj->GetLocalScale();
	Vector3f scale = parentScale.EntrywiseMultiply( localScale );

	OVR_ASSERT( obj != NULL );

/*
	VRMenuObject const * parent = ToObject( obj->GetParentHandle() );
	if ( parent != NULL )
	{
		fontParms_t fontParms;
		Vector3f itemUp = curModelPose.Orientation * Vector3f( 0.0f, 1.0f, 0.0f );
		Vector3f itemNormal = curModelPose.Orientation * Vector3f( 0.0f, 0.0f, 1.0f );
		fontSurface.DrawText3D( font, fontParms, curModelPose.Position, itemNormal, itemUp, 
				1.0f, Vector4f( 1.0f, 0.0f, 1.0f, 1.0f ), parent->GetText().ToCStr() );
	}
*/
	if ( obj->GetType() != VRMENU_CONTAINER )	// containers never render, but their children may
	{
        Posef const & hilightPose = obj->GetHilightPose();
        Posef itemPose( curModelPose.Orientation * hilightPose.Orientation,
                        curModelPose.Position + ( curModelPose.Orientation * parentScale.EntrywiseMultiply( hilightPose.Position ) ) );
		Matrix4f poseMat( itemPose.Orientation );
		Vector3f itemUp = poseMat.GetYBasis();
		Vector3f itemNormal = poseMat.GetZBasis();
		curModelPose = itemPose;	// so children like the slider bar caret use our hilight offset and don't end up clipping behind us!
		VRMenuRenderFlags_t rFlags = flags;
		VRMenuObjectFlags_t oFlags = obj->GetFlags();
		if ( oFlags & VRMENUOBJECT_FLAG_POLYGON_OFFSET )
		{
			rFlags |= VRMENU_RENDER_POLYGON_OFFSET;
		}
		if ( oFlags & VRMENUOBJECT_FLAG_NO_DEPTH )
		{
			rFlags |= VRMENU_RENDER_NO_DEPTH;
		}

		// the menu object may have zero or more renderable surfaces (if 0, it may draw only text)
		Array< VRMenuSurface > const & surfaces = obj->GetSurfaces();
		for ( int i = 0; i < surfaces.GetSizeI(); ++i )
		{
			VRMenuSurface const & surf = surfaces[i];
			if ( surf.IsRenderable() )
			{
				SubmittedMenuObject & sub = submitted[curIndex];
				sub.SurfaceIndex = i;
				sub.Pose = itemPose;
				sub.Scale = scale;
				sub.Flags = rFlags;
				sub.ColorTableOffset = obj->GetColorTableOffset();
				sub.SkipAdditivePass = !obj->IsHilighted();
				sub.Handle = obj->GetHandle();
				// modulate surface color with parent's current color
				sub.Color = surf.GetColor() * curColor;
				sub.Offsets = surf.GetAnchorOffsets();
				sub.FadeDirection = obj->GetFadeDirection();
#if defined( OVR_BUILD_DEBUG )
				sub.SurfaceName = surf.GetName();
#endif
				curIndex++;
			}
		}

		OVR::String const & text = obj->GetText();
		if ( ( oFlags & VRMENUOBJECT_DONT_RENDER_TEXT ) == 0 && text.GetLengthI() > 0 )
		{
            Posef const & textLocalPose = obj->GetTextLocalPose();
            Posef curTextPose;
            curTextPose.Position = itemPose.Position + ( itemPose.Orientation * textLocalPose.Position * scale );
            curTextPose.Orientation = textLocalPose.Orientation * itemPose.Orientation;
            Vector3f textNormal = curTextPose.Orientation * Vector3f( 0.0f, 0.0f, 1.0f );
			Vector3f position = curTextPose.Position + textNormal * 0.001f; // this is simply to prevent z-fighting right now
            Vector3f textScale = scale * obj->GetTextLocalScale();

            Vector4f textColor = obj->GetTextColor();
            // Apply parent's alpha influence
            textColor.w *= parentColor.w;
			VRMenuFontParms const & fp = obj->GetFontParms();
			fontParms_t fontParms;
			fontParms.CenterHoriz = fp.CenterHoriz;
			fontParms.CenterVert = fp.CenterVert;
			fontParms.Billboard = fp.Billboard;
			fontParms.TrackRoll = fp.TrackRoll;
			fontParms.ColorCenter = fp.ColorCenter;
			fontParms.AlphaCenter = fp.AlphaCenter;

			fontSurface.DrawText3D( font, fontParms, position, itemNormal, itemUp, 
					textScale.x * fp.Scale, textColor, text.ToCStr() );
#if 0
			// this shows a ruler for the wrap width when rendering text
			float const wrapw = obj->GetWrapWidth();
			Vector3f pos = position + Vector3f( 0.0f, 0.0f, -0.1f );
			debugLines.AddLine( pos - wrapw * 0.5f * scale, pos + wrapw * 0.5f * scale,
				Vector4f( 0.0f, 1.0f, 0.0f, 1.0f ), Vector4f( 1.0f, 0.0f, 0.0f, 1.0f ), 0, false );
#endif
		}
        //DROIDLOG( "Spam", "AddPoint for '%s'", text.ToCStr() );
		//GetDebugLines().AddPoint( curModelPose.Position, 0.05f, 1, true );
	}

    cullBounds = obj->GetLocalBounds( font ) * parentScale;

	// submit all children
    if ( obj->Children.GetSizeI() > 0 )
    {
	    for ( int i = 0; i < obj->Children.GetSizeI(); ++i )
	    {
		    menuHandle_t childHandle = obj->Children[i];
		    VRMenuObjectLocal const * child = static_cast< VRMenuObjectLocal const * >( ToObject( childHandle ) );
		    if ( child == NULL )
		    {
			    continue;
		    }

            Bounds3f childCullBounds;
		    SubmitForRenderingRecursive( debugLines, font, fontSurface, flags, child, curModelPose, curColor, scale, 
                    childCullBounds, submitted, maxIndices, curIndex );

		    Posef pose = child->GetLocalPose();
		    pose.Position = pose.Position * scale;
            childCullBounds = Bounds3f::Transform( pose, childCullBounds );
            cullBounds = Bounds3f::Union( cullBounds, childCullBounds );
	    }
    }

    obj->SetCullBounds( cullBounds );

#if 0
	OvrCollisionPrimitive const * cp = obj->GetCollisionPrimitive();
	if ( cp != NULL )
	{
		cp->DebugRender( debugLines, curModelPose );
	}
    {
        // for debug drawing, put the cull bounds in world space
        //LogBounds( obj->GetText().ToCStr(), "Transformed CullBounds", myCullBounds );
        debugLines.AddBounds( curModelPose, obj->GetCullBounds(), Vector4f( 0.0f, 1.0f, 1.0f, 1.0f ) );
    }
    {
        Bounds3f localBounds = obj->GetLocalBounds( font ) * parentScale;
        //LogBounds( obj->GetText().ToCStr(), "localBounds", localBounds );
    	debugLines.AddBounds( curModelPose, localBounds, Vector4f( 1.0f, 0.0f, 0.0f, 1.0f ) );
        Bounds3f textLocalBounds = obj->GetTextLocalBounds( font );
        Posef hilightPose = obj->GetHilightPose();
        textLocalBounds = Bounds3f::Transform( Posef( hilightPose.Orientation, hilightPose.Position * scale ), textLocalBounds );
    	debugLines.AddBounds( curModelPose, textLocalBounds * parentScale, Vector4f( 1.0f, 1.0f, 0.0f, 1.0f ) );
    }
#endif
}
Ejemplo n.º 17
0
Ray Camera::get_primary_ray_dof(float x, float y, float width, float height)
{
	// Primary ray
	Ray pray, fray;

	// Take the aspect ratio into consideration.
	scalar_t ratio = (scalar_t)width / (scalar_t)height;

	// Set the primary ray's origin at the camera's position.
	pray.origin = position;

	// Calculate the ray's intersection point on the projection plane.
	pray.direction.x = (2.0 * (scalar_t)x / (scalar_t)width) - 1.0;
	pray.direction.y = ((2.0 * (scalar_t)y / (scalar_t)height) - 1.0) / ratio;
	pray.direction.z = 1.0 / tan(fov * NMath::RADIAN / 2.0);

	// Calculate the deviated ray direction
	fray.origin = pray.direction;
	scalar_t half_aperture = aperture / 2.f;
	fray.origin.x += NMath::prng_c(-half_aperture, half_aperture);
	fray.origin.y += NMath::prng_c(-half_aperture, half_aperture);

	// Find the intersection point on the focal plane
	Vector3f fpip = pray.direction + flength * pray.direction.normalized();
	fray.direction = fpip - fray.origin;

	/*
		Setting up the look-at matrix is easy when you consider that a matrix
		is basically a rotated unit cube formed by three vectors (the 3x3 part) at a
		particular position (the 1x3 part).

		We already have one of the three vectors:
			-	The z-axis of the matrix is simply the view direction.
			-	The x-axis of the matrix is a bit tricky: if the camera is not tilted,
				then the x-axis of the matrix is perpendicular to the z-axis and
				the vector (0, 1, 0).
			-	The y-axis is perpendicular to the other two, so we simply calculate
				the cross product of the x-axis and the z-axis to obtain the y-axis.
				Note that the y-axis is calculated using the reversed z-axis. The
				image will be upside down without this adjustment.
	*/

	// Calculate the camera direction vector and normalize it.
	Vector3f camdir = target - position;
	camdir.normalize();

	Vector3f rx,ry,rz;

	rz = camdir;
	rx = cross(up, rz);
	rx.normalize();
	ry = cross(rx, rz);
	ry.normalize();

	Matrix4x4f tmat(rx.x, ry.x, rz.x, 0,
					rx.y, ry.y, rz.y, 0,
					rx.z, ry.z, rz.z, 0,
					0, 0, 0, 1);

	// Transform the direction vector
	fray.direction.transform(tmat);
	fray.direction.normalize();

	// Transform the origin of the ray
	fray.origin.transform(tmat);
	fray.origin += position;

	return fray;
}
Ejemplo n.º 18
0
	void Update(const Phoenix::Camera &cam)
	{
		glClearColor(0,0,0,0);
		// Cheap collision detection/response
		Vector3f vCamera = cam.getPosition();
		/*
		if(vCamera.magnitude() < m_fInnerRadius + 0.01f)
		{
			Vector3f N = vCamera / vCamera.Magnitude();
			Vector3f I = CCameraTask::GetPtr()->GetVelocity();
			float fSpeed = I.magnitude();
			I /= fSpeed;
			Vector3f R = N * (2.0*(-I | N)) + I;
			CCameraTask::GetPtr()->SetVelocity(R * fSpeed);

			vCamera = N * (m_fInnerRadius + 0.01f);
			CCameraTask::GetPtr()->SetPosition(vCamera);
		}
		*/
		//Vector3f vUnitCamera = vCamera / vCamera.magnitude();

		glPolygonMode(GL_FRONT, m_nPolygonMode);

		// Draw the groud sphere
		CGLShaderObject *pGroundShader;
		if(vCamera.magnitude() >= m_fOuterRadius)
			pGroundShader = &m_shGroundFromSpace;
		else
			pGroundShader = &m_shGroundFromAtmosphere;
		pGroundShader->Enable();
		pGroundShader->SetUniformParameter3f("v3CameraPos", vCamera.x, vCamera.y, vCamera.z);
		pGroundShader->SetUniformParameter3f("v3LightPos", m_vLightDirection.x, m_vLightDirection.y, m_vLightDirection.z);
		pGroundShader->SetUniformParameter3f("v3InvWavelength", 1/m_fWavelength4[0], 1/m_fWavelength4[1], 1/m_fWavelength4[2]);
		pGroundShader->SetUniformParameter1f("fCameraHeight", vCamera.magnitude());
		pGroundShader->SetUniformParameter1f("fCameraHeight2", vCamera.magnitudeSquared());
		pGroundShader->SetUniformParameter1f("fInnerRadius", m_fInnerRadius);
		pGroundShader->SetUniformParameter1f("fInnerRadius2", m_fInnerRadius*m_fInnerRadius);
		pGroundShader->SetUniformParameter1f("fOuterRadius", m_fOuterRadius);
		pGroundShader->SetUniformParameter1f("fOuterRadius2", m_fOuterRadius*m_fOuterRadius);
		pGroundShader->SetUniformParameter1f("fKrESun", m_Kr*m_ESun);
		pGroundShader->SetUniformParameter1f("fKmESun", m_Km*m_ESun);
		pGroundShader->SetUniformParameter1f("fKr4PI", m_Kr4PI);
		pGroundShader->SetUniformParameter1f("fKm4PI", m_Km4PI);
		pGroundShader->SetUniformParameter1f("fScale", 1.0f / (m_fOuterRadius - m_fInnerRadius));
		pGroundShader->SetUniformParameter1f("fScaleDepth", m_fRayleighScaleDepth);
		pGroundShader->SetUniformParameter1f("fScaleOverScaleDepth", (1.0f / (m_fOuterRadius - m_fInnerRadius)) / m_fRayleighScaleDepth);
		pGroundShader->SetUniformParameter1f("g", m_g);
		pGroundShader->SetUniformParameter1f("g2", m_g*m_g);
		pGroundShader->SetUniformParameter1i("nSamples", m_nSamples);
		pGroundShader->SetUniformParameter1f("fSamples", m_nSamples);
		//pGroundShader->SetUniformParameter1i("s2Test", 0);
		GLUquadricObj *pSphere = gluNewQuadric();
		//m_tEarth.Enable();
		gluSphere(pSphere, m_fInnerRadius, 100, 50);
		//m_tEarth.Disable();
		gluDeleteQuadric(pSphere);
		pGroundShader->Disable();

		// Draw the sky sphere
		CGLShaderObject *pSkyShader;
		if(vCamera.magnitude() >= m_fOuterRadius)
			pSkyShader = &m_shSkyFromSpace;
		else
			pSkyShader = &m_shSkyFromAtmosphere;
		pSkyShader->Enable();
		pSkyShader->SetUniformParameter3f("v3CameraPos", vCamera.x, vCamera.y, vCamera.z);
		pSkyShader->SetUniformParameter3f("v3LightPos", m_vLightDirection.x, m_vLightDirection.y, m_vLightDirection.z);
		pSkyShader->SetUniformParameter3f("v3InvWavelength", 1/m_fWavelength4[0], 1/m_fWavelength4[1], 1/m_fWavelength4[2]);
		pSkyShader->SetUniformParameter1f("fCameraHeight", vCamera.magnitude());
		pSkyShader->SetUniformParameter1f("fCameraHeight2", vCamera.magnitudeSquared());
		pSkyShader->SetUniformParameter1f("fInnerRadius", m_fInnerRadius);
		pSkyShader->SetUniformParameter1f("fInnerRadius2", m_fInnerRadius*m_fInnerRadius);
		pSkyShader->SetUniformParameter1f("fOuterRadius", m_fOuterRadius);
		pSkyShader->SetUniformParameter1f("fOuterRadius2", m_fOuterRadius*m_fOuterRadius);
		pSkyShader->SetUniformParameter1f("fKrESun", m_Kr*m_ESun);
		pSkyShader->SetUniformParameter1f("fKmESun", m_Km*m_ESun);
		pSkyShader->SetUniformParameter1f("fKr4PI", m_Kr4PI);
		pSkyShader->SetUniformParameter1f("fKm4PI", m_Km4PI);
		pSkyShader->SetUniformParameter1f("fScale", 1.0f / (m_fOuterRadius - m_fInnerRadius));
		pSkyShader->SetUniformParameter1f("fScaleDepth", m_fRayleighScaleDepth);
		pSkyShader->SetUniformParameter1f("fScaleOverScaleDepth", (1.0f / (m_fOuterRadius - m_fInnerRadius)) / m_fRayleighScaleDepth);
		pSkyShader->SetUniformParameter1f("g", m_g);
		pSkyShader->SetUniformParameter1f("g2", m_g*m_g);
		pSkyShader->SetUniformParameter1i("nSamples", m_nSamples);
		pSkyShader->SetUniformParameter1f("fSamples", m_nSamples);
		//m_tOpticalDepth.Enable();
		//pSkyShader->SetUniformParameter1f("tex", 0);
		glFrontFace(GL_CW);
		glEnable(GL_BLEND);
		glBlendFunc(GL_ONE, GL_ONE);
		pSphere = gluNewQuadric();
		gluSphere(pSphere, m_fOuterRadius, 100, 100);
		gluDeleteQuadric(pSphere);
		glDisable(GL_BLEND);
		glFrontFace(GL_CCW);
		//m_tOpticalDepth.Disable();
		pSkyShader->Disable();

		glPolygonMode(GL_FRONT, GL_FILL);
	}
Ejemplo n.º 19
0
// perform pre-arm checks and set ap.pre_arm_check flag
//  return true if the checks pass successfully
bool Copter::pre_arm_checks(bool display_failure)
{
    // exit immediately if already armed
    if (motors.armed()) {
        return true;
    }

    // check if motor interlock and Emergency Stop aux switches are used
    // at the same time.  This cannot be allowed.
    if (check_if_auxsw_mode_used(AUXSW_MOTOR_INTERLOCK) && check_if_auxsw_mode_used(AUXSW_MOTOR_ESTOP)){
        if (display_failure) {
            gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Interlock/E-Stop Conflict");
        }
        return false;
    }

    // check if motor interlock aux switch is in use
    // if it is, switch needs to be in disabled position to arm
    // otherwise exit immediately.  This check to be repeated,
    // as state can change at any time.
    if (ap.using_interlock && motors.get_interlock()){
        if (display_failure) {
            gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Motor Interlock Enabled");
        }
        return false;
    }

    // exit immediately if we've already successfully performed the pre-arm check
    if (ap.pre_arm_check) {
        // run gps checks because results may change and affect LED colour
        // no need to display failures because arm_checks will do that if the pilot tries to arm
        pre_arm_gps_checks(false);
        return true;
    }

    // succeed if pre arm checks are disabled
    if (g.arming_check == ARMING_CHECK_NONE) {
        set_pre_arm_check(true);
        set_pre_arm_rc_check(true);
        return true;
    }

    // pre-arm rc checks a prerequisite
    pre_arm_rc_checks();
    if (!ap.pre_arm_rc_check) {
        if (display_failure) {
            gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: RC not calibrated");
        }
        return false;
    }
    // check Baro
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_BARO)) {
        // barometer health check
        if (!barometer.all_healthy()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Barometer not healthy");
            }
            return false;
        }
        // Check baro & inav alt are within 1m if EKF is operating in an absolute position mode.
        // Do not check if intending to operate in a ground relative height mode as EKF will output a ground relative height
        // that may differ from the baro height due to baro drift.
        nav_filter_status filt_status = inertial_nav.get_filter_status();
        bool using_baro_ref = (!filt_status.flags.pred_horiz_pos_rel && filt_status.flags.pred_horiz_pos_abs);
        if (using_baro_ref) {
            if (fabsf(inertial_nav.get_altitude() - baro_alt) > PREARM_MAX_ALT_DISPARITY_CM) {
                if (display_failure) {
                    gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Altitude disparity");
                }
                return false;
            }
        }
    }

    // check Compass
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_COMPASS)) {
        //check if compass has calibrated and requires reboot
        if (compass.compass_cal_requires_reboot()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Compass calibrated requires reboot");
            }
            return false;
        }

        // check the primary compass is healthy
        if (!compass.healthy()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not healthy");
            }
            return false;
        }

        // check compass learning is on or offsets have been set
        if (!compass.configured()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass not calibrated");
            }
            return false;
        }

        // check for unreasonable compass offsets
        Vector3f offsets = compass.get_offsets();
        if (offsets.length() > COMPASS_OFFSETS_MAX) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Compass offsets too high");
            }
            return false;
        }

        // check for unreasonable mag field length
        float mag_field = compass.get_field().length();
        if (mag_field > COMPASS_MAGFIELD_EXPECTED*1.65f || mag_field < COMPASS_MAGFIELD_EXPECTED*0.35f) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check mag field");
            }
            return false;
        }

        // check all compasses point in roughly same direction
        if (!compass.consistent()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent compasses");
            }
            return false;
        }

    }

    // check GPS
    if (!pre_arm_gps_checks(display_failure)) {
        return false;
    }

    #if AC_FENCE == ENABLED
    // check fence is initialised
    if (!fence.pre_arm_check()) {
        if (display_failure) {
            gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check fence");
        }
        return false;
    }
    #endif

    // check INS
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_INS)) {
        // check accelerometers have been calibrated
        if (!ins.accel_calibrated_ok_all()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accels not calibrated");
            }
            return false;
        }

        // check accels are healthy
        if (!ins.get_accel_health_all()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Accelerometers not healthy");
            }
            return false;
        }

        //check if accelerometers have calibrated and require reboot
        if (ins.accel_cal_requires_reboot()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL, "PreArm: Accelerometers calibrated requires reboot");
            }
            return false;
        }

        // check all accelerometers point in roughly same direction
        if (ins.get_accel_count() > 1) {
            const Vector3f &prime_accel_vec = ins.get_accel();
            for(uint8_t i=0; i<ins.get_accel_count(); i++) {
                // get next accel vector
                const Vector3f &accel_vec = ins.get_accel(i);
                Vector3f vec_diff = accel_vec - prime_accel_vec;
                float threshold = PREARM_MAX_ACCEL_VECTOR_DIFF;
                if (i >= 2) {
                    /*
                     * for boards with 3 IMUs we only use the first two
                     * in the EKF. Allow for larger accel discrepancy
                     * for IMU3 as it may be running at a different temperature
                     */
                    threshold *= 2;
                }
                if (vec_diff.length() > threshold) {
                    if (display_failure) {
                        gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Accelerometers");
                    }
                    return false;
                }
            }
        }

        // check gyros are healthy
        if (!ins.get_gyro_health_all()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Gyros not healthy");
            }
            return false;
        }

        // check all gyros are consistent
        if (ins.get_gyro_count() > 1) {
            for(uint8_t i=0; i<ins.get_gyro_count(); i++) {
                // get rotation rate difference between gyro #i and primary gyro
                Vector3f vec_diff = ins.get_gyro(i) - ins.get_gyro();
                if (vec_diff.length() > PREARM_MAX_GYRO_VECTOR_DIFF) {
                    if (display_failure) {
                        gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: inconsistent Gyros");
                    }
                    return false;
                }
            }
        }

        // get ekf attitude (if bad, it's usually the gyro biases)
        if (!pre_arm_ekf_attitude_check()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: gyros still settling");
            }
            return false;
        }
    }
    #if CONFIG_HAL_BOARD != HAL_BOARD_VRBRAIN
    #ifndef CONFIG_ARCH_BOARD_PX4FMU_V1
    // check board voltage
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
        if (hal.analogin->board_voltage() < BOARD_VOLTAGE_MIN || hal.analogin->board_voltage() > BOARD_VOLTAGE_MAX) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Board Voltage");
            }
            return false;
        }
    }
    #endif
    #endif

    // check battery voltage
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_VOLTAGE)) {
        if (failsafe.battery || (!ap.usb_connected && battery.exhausted(g.fs_batt_voltage, g.fs_batt_mah))) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check Battery");
            }
            return false;
        }
    }

    // check various parameter values
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_PARAMETERS)) {

        // ensure ch7 and ch8 have different functions
        if (check_duplicate_auxsw()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Duplicate Aux Switch Options");
            }
            return false;
        }

        // failsafe parameter checks
        if (g.failsafe_throttle) {
            // check throttle min is above throttle failsafe trigger and that the trigger is above ppm encoder's loss-of-signal value of 900
            if (channel_throttle->radio_min <= g.failsafe_throttle_value+10 || g.failsafe_throttle_value < 910) {
                if (display_failure) {
                    gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check FS_THR_VALUE");
                }
                return false;
            }
        }

        // lean angle parameter check
        if (aparm.angle_max < 1000 || aparm.angle_max > 8000) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Check ANGLE_MAX");
            }
            return false;
        }

        // acro balance parameter check
        if ((g.acro_balance_roll > attitude_control.get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control.get_angle_pitch_p().kP())) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
            }
            return false;
        }

        #if CONFIG_SONAR == ENABLED && OPTFLOW == ENABLED
        // check range finder if optflow enabled
        if (optflow.enabled() && !sonar.pre_arm_check()) {
            if (display_failure) {
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check range finder");
            }
            return false;
        }
        #endif
        #if FRAME_CONFIG == HELI_FRAME
        // check helicopter parameters
        if (!motors.parameter_check(display_failure)) {
            return false;
        }
        #endif // HELI_FRAME

        // check for missing terrain data
        if (!pre_arm_terrain_check()) {
            gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Waiting for Terrain data");
            return false;
        }
    }

    // check throttle is above failsafe throttle
    // this is near the bottom to allow other failures to be displayed before checking pilot throttle
    if ((g.arming_check == ARMING_CHECK_ALL) || (g.arming_check & ARMING_CHECK_RC)) {
        if (g.failsafe_throttle != FS_THR_DISABLED && channel_throttle->radio_in < g.failsafe_throttle_value) {
            if (display_failure) {
                #if FRAME_CONFIG == HELI_FRAME
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Collective below Failsafe");
                #else
                gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: Throttle below Failsafe");
                #endif
            }
            return false;
        }
    }

    return true;
}
Ejemplo n.º 20
0
void Cube::update(const Vector3f &min, const Vector3f &max) {
	std::string v =
	std::to_string(max.x()) + " " + std::to_string(min.y()) + " " + std::to_string(min.z()) + "\n" +
	std::to_string(max.x()) + " " + std::to_string(min.y()) + " " + std::to_string(max.z()) + "\n" +
	std::to_string(min.x()) + " " + std::to_string(min.y()) + " " + std::to_string(max.z()) + "\n" +
	std::to_string(min.x()) + " " + std::to_string(min.y()) + " " + std::to_string(min.z()) + "\n" +
	std::to_string(max.x()) + " " + std::to_string(max.y()) + " " + std::to_string(min.z()) + "\n" +
	std::to_string(max.x()) + " " + std::to_string(max.y()) + " " + std::to_string(max.z()) + "\n" +
	std::to_string(min.x()) + " " + std::to_string(max.y()) + " " + std::to_string(max.z()) + "\n" +
	std::to_string(min.x()) + " " + std::to_string(max.y()) + " " + std::to_string(min.z()) + "\n";

	std::istringstream is(v);
	std::string line_str;
	std::vector<Vector3f> newPos;
	m_bbox.reset();
	while (std::getline(is, line_str)) {
		std::istringstream line(line_str);
		
		Point3f p;
		line >> p.x() >> p.y() >> p.z();
		newPos.push_back(p);
		m_bbox.expandBy(p);
	}

	for (uint32_t i = 0; i < m_V.cols(); ++i)
		m_V.col(i) = newPos.at(vertices[i].p - 1);

	// Update data on GPU
	glBindVertexArray(vao);
	glBindBuffer(GL_ARRAY_BUFFER, vbo[VERTEX_BUFFER]);
	// Update data only
	void* ptr = glMapBuffer(GL_ARRAY_BUFFER, GL_WRITE_ONLY);
	if (ptr) {
		memcpy(ptr, (const uint8_t *)m_V.data(), 3 * m_V.cols() * sizeof(GLfloat));
		glUnmapBuffer(GL_ARRAY_BUFFER);
	}
	glBindBuffer(GL_ARRAY_BUFFER, 0);
	glBindVertexArray(0);
}
Ejemplo n.º 21
0
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Clamp( const Vector3f& A )
{
    Vector3f vec = A;
    vec.Clamp();
    return vec;
}
Ejemplo n.º 22
0
traceResult_t RtTrace::Trace( const Vector3f & start, const Vector3f & end ) const
{
	traceResult_t result;
	result.triangleIndex = -1;
	result.fraction = 1.0f;
	result.uv = Vector2f( 0.0f );
	result.normal = Vector3f( 0.0f );

	const Vector3f rayDelta = end - start;
	const float rayLengthSqr = rayDelta.LengthSq();
	const float rayLengthRcp = RcpSqrt( rayLengthSqr );
	const float rayLength = rayLengthSqr * rayLengthRcp;
	const Vector3f rayDir = rayDelta * rayLengthRcp;

	const float rcpRayDirX = ( fabsf( rayDir.x ) > Math<float>::SmallestNonDenormal ) ? ( 1.0f / rayDir.x ) : Math<float>::HugeNumber;
	const float rcpRayDirY = ( fabsf( rayDir.y ) > Math<float>::SmallestNonDenormal ) ? ( 1.0f / rayDir.y ) : Math<float>::HugeNumber;
	const float rcpRayDirZ = ( fabsf( rayDir.z ) > Math<float>::SmallestNonDenormal ) ? ( 1.0f / rayDir.z ) : Math<float>::HugeNumber;

	const float sX = ( header.bounds.GetMins()[0] - start.x ) * rcpRayDirX;
	const float sY = ( header.bounds.GetMins()[1] - start.y ) * rcpRayDirY;
	const float sZ = ( header.bounds.GetMins()[2] - start.z ) * rcpRayDirZ;

	const float tX = ( header.bounds.GetMaxs()[0] - start.x ) * rcpRayDirX;
	const float tY = ( header.bounds.GetMaxs()[1] - start.y ) * rcpRayDirY;
	const float tZ = ( header.bounds.GetMaxs()[2] - start.z ) * rcpRayDirZ;

	const float minX = Alg::Min( sX, tX );
	const float minY = Alg::Min( sY, tY );
	const float minZ = Alg::Min( sZ, tZ );

	const float maxX = Alg::Max( sX, tX );
	const float maxY = Alg::Max( sY, tY );
	const float maxZ = Alg::Max( sZ, tZ );

	const float t0 = Alg::Max( minX, Alg::Max( minY, minZ ) );
	const float t1 = Alg::Min( maxX, Alg::Min( maxY, maxZ ) );

	if ( t0 >= t1 )
	{
		return result;
	}

	float entryDistance = Alg::Max( t0, 0.0f );
	float bestDistance = Alg::Min( t1 + 0.00001f, rayLength );
	Vector2f uv;
	
	const kdtree_node_t * currentNode = &nodes[0];
	
	for ( int i = 0; i < RT_KDTREE_MAX_ITERATIONS; i++ )
	{
		const Vector3f rayEntryPoint = start + rayDir * entryDistance;

		// Step down the tree until a leaf node is found.
		while ( ( currentNode->data & 1 ) == 0 )
		{
			// Select the child node based on whether the entry point is left or right of the split plane.
			// If the entry point is directly at the split plane then choose the side based on the ray direction.
			const int nodePlane = ( ( currentNode->data >> 1 ) & 3 );
			int child;
			if ( rayEntryPoint[nodePlane] - currentNode->dist < 0.00001f ) child = 0;
			else if ( rayEntryPoint[nodePlane] - currentNode->dist > 0.00001f ) child = 1;
			else child = ( rayDelta[nodePlane] > 0.0f );
			currentNode = &nodes[( currentNode->data >> 3 ) + child];
		}

		// Check for an intersection with a triangle in this leaf.
		const kdtree_leaf_t * currentLeaf = &leafs[( currentNode->data >> 3 )];
		const int * leafTriangles = currentLeaf->triangles;
		int leafTriangleCount = RT_KDTREE_MAX_LEAF_TRIANGLES;
		for ( int j = 0; j < leafTriangleCount; j++ )
		{
			int currentTriangle = leafTriangles[j];
			if ( currentTriangle < 0 )
			{
				if ( currentTriangle == -1 )
				{
					break;
				}

				const int offset = ( currentTriangle & 0x7FFFFFFF );
				leafTriangles = &overflow[offset];
				leafTriangleCount = header.numOverflow - offset;
				j = 0;
				currentTriangle = leafTriangles[0];
			}

			float distance;
			float u;
			float v;

			if ( RtIntersect::RayTriangle( start, rayDir,
										vertices[indices[currentTriangle * 3 + 0]],
										vertices[indices[currentTriangle * 3 + 1]],
										vertices[indices[currentTriangle * 3 + 2]], distance, u, v ) )
			{
				if ( distance >= 0.0f && distance < bestDistance )
				{
					bestDistance = distance;

					result.triangleIndex = currentTriangle * 3;
					uv.x = u;
					uv.y = v;
				}
			}
		}

		// Calculate the distance along the ray where the next leaf is entered.
		const float sX = ( currentLeaf->bounds.GetMins()[0] - start.x ) * rcpRayDirX;
		const float sY = ( currentLeaf->bounds.GetMins()[1] - start.y ) * rcpRayDirY;
		const float sZ = ( currentLeaf->bounds.GetMins()[2] - start.z ) * rcpRayDirZ;

		const float tX = ( currentLeaf->bounds.GetMaxs()[0] - start.x ) * rcpRayDirX;
		const float tY = ( currentLeaf->bounds.GetMaxs()[1] - start.y ) * rcpRayDirY;
		const float tZ = ( currentLeaf->bounds.GetMaxs()[2] - start.z ) * rcpRayDirZ;

		const float maxX = Alg::Max( sX, tX );
		const float maxY = Alg::Max( sY, tY );
		const float maxZ = Alg::Max( sZ, tZ );

		entryDistance = Alg::Min( maxX, Alg::Min( maxY, maxZ ) );
		if ( entryDistance >= bestDistance )
		{
			break;
		}

		// Calculate the exit plane.
		const int exitX = ( 0 << 1 ) | ( ( sX < tX ) ? 1 : 0 );
		const int exitY = ( 1 << 1 ) | ( ( sY < tY ) ? 1 : 0 );
		const int exitZ = ( 2 << 1 ) | ( ( sZ < tZ ) ? 1 : 0 );
		const int exitPlane = ( maxX < maxY ) ? ( maxX < maxZ ? exitX : exitZ ) : ( maxY < maxZ ? exitY : exitZ );

		// Use a rope to enter the adjacent leaf.
		const int exitNodeIndex = currentLeaf->ropes[exitPlane];
		if ( exitNodeIndex == -1 )
		{
			break;
		}

		currentNode = &nodes[exitNodeIndex];
	}

	if ( result.triangleIndex != -1 )
	{
		result.fraction = bestDistance * rayLengthRcp;
		result.uv = uvs[indices[result.triangleIndex + 0]] * ( 1.0f - uv.x - uv.y ) +
					uvs[indices[result.triangleIndex + 1]] * uv.x +
					uvs[indices[result.triangleIndex + 2]] * uv.y;
		const Vector3f d1 = vertices[indices[result.triangleIndex + 1]] - vertices[indices[result.triangleIndex + 0]];
		const Vector3f d2 = vertices[indices[result.triangleIndex + 2]] - vertices[indices[result.triangleIndex + 0]];
		result.normal = d1.Cross( d2 ).Normalized();
	}

	return result;
}
Ejemplo n.º 23
0
//----------------------------------------------------------------------------------------------------
float Vector3f::Dot( const Vector3f& A, const Vector3f& B )
{    
    return A.Dot( B );
}
Ejemplo n.º 24
0
/**
 * Compute the scale factor and bias associated with a vector observer
 * according to the equation:
 * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k)
 * where k is the sample index,
 *       B_k is the kth measurement
 *       I_{3x3} is a 3x3 identity matrix
 *       D is a 3x3 diagonal matrix of scale factors
 *       O is the orthogonal alignment matrix
 *       A_k is the attitude at the kth sample
 *       b is the bias in the global reference frame
 *       \epsilon_k is the noise at the kth sample
 * This implementation makes the assumption that \epsilon is a constant white,
 * gaussian noise source that is common to all k.  The misalignment matrix O
 * is not computed, and the off-diagonal elements of D, corresponding to the
 * misalignment scale factors are not either.
 *
 * @param bias[out] The computed bias, in the global frame
 * @param scale[out] The computed scale factor, in the sensor frame
 * @param samples[in] An array of measurement samples.
 * @param n_samples The number of samples in the array.
 * @param referenceField The magnetic field vector at this location.
 * @param noise The one-sigma squared standard deviation of the observed noise
 * in the sensor.
 */
void twostep_bias_scale(Vector3f & bias,
                        Vector3f & scale,
                        const Vector3f samples[],
                        const size_t n_samples,
                        const Vector3f & referenceField,
                        const float noise)
{
    // Initial estimate for gradiant descent starts at eq 37a of ref 2.

    // Define L_k by eq 30 and 28 for k = 1 .. n_samples
    Matrix<double, Dynamic, 6> fullSamples(n_samples, 6);
    // \hbar{L} by eq 33, simplified by obesrving that the
    Matrix<double, 1, 6> centerSample = Matrix<double, 1, 6>::Zero();
    // Define the sample differences z_k by eq 23 a)
    double sampleDeltaMag[n_samples];
    // The center value \hbar{z}
    double sampleDeltaMagCenter = 0;
    double refSquaredNorm = referenceField.squaredNorm();

    for (size_t i = 0; i < n_samples; ++i) {
        fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(),
            -(samples[i][0] * samples[i][0]),
            -(samples[i][1] * samples[i][1]),
            -(samples[i][2] * samples[i][2]);
        centerSample += fullSamples.row(i);

        sampleDeltaMag[i]     = samples[i].squaredNorm() - refSquaredNorm;
        sampleDeltaMagCenter += sampleDeltaMag[i];
    }
    sampleDeltaMagCenter /= n_samples;
    centerSample /= n_samples;

    // True for all k.
    // double mu = -3*noise;
    // The center value of mu, \bar{mu}
    // double centerMu = mu;
    // The center value of mu, \tilde{mu}
    // double centeredMu = 0;

    // Define \tilde{L}_k for k = 0 .. n_samples
    Matrix<double, Dynamic, 6> centeredSamples(n_samples, 6);
    // Define \tilde{z}_k for k = 0 .. n_samples
    double centeredMags[n_samples];
    // Compute the term under the summation of eq 37a
    Matrix<double, 6, 1> estimateSummation = Matrix<double, 6, 1>::Zero();
    for (size_t i = 0; i < n_samples; ++i) {
        centeredSamples.row(i) = fullSamples.row(i) - centerSample;
        centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
        estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose();
    }
    estimateSummation /= noise; // note: paper supplies 1/noise

    // By eq 37 b).  Note, paper supplies 1/noise here
    Matrix<double, 6, 6> P_theta_theta_inv = (1.0f / noise) *
                                             centeredSamples.transpose() * centeredSamples;
#ifdef PRINTF_DEBUGGING
    SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig(P_theta_theta_inv);
    std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n";
    std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose()
              << "\n";
    std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n";
#endif

    // The Fisher information matrix has a small eigenvalue, with a
    // corresponding eigenvector of about [1e-2 1e-2 1e-2 0.55, 0.55,
    // 0.55].  This means that there is relatively little information
    // about the common gain on the scale factor, which has a
    // corresponding effect on the bias, too. The fixup is performed by
    // the first iteration of the second stage of TWOSTEP, as documented in
    // [1].
    Matrix<double, 6, 1> estimate;
    // By eq 37 a), \tilde{Fisher}^-1
    P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);

#ifdef PRINTF_DEBUGGING
    Matrix<double, 6, 6> P_theta_theta;
    P_theta_theta_inv.ldlt().solve(Matrix<double, 6, 6>::Identity(), &P_theta_theta);
    SelfAdjointEigenSolver<Matrix<double, 6, 6> > eig2(P_theta_theta);
    std::cout << "eigenvalues: " << eig2.eigenvalues().transpose() << "\n";
    std::cout << "eigenvectors: \n" << eig2.eigenvectors() << "\n";
    std::cout << "estimate summation: \n" << estimateSummation.normalized()
              << "\n";
#endif

    // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradiant(theta)
    // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1}
    size_t count = 3;
    while (count-- > 0) {
        // eq 40
        Matrix<double, 1, 6> db_dtheta = dnormb_dtheta(estimate);

        Matrix<double, 6, 1> dJ_dtheta = dJdb(centerSample,
                                              sampleDeltaMagCenter,
                                              estimate,
                                              db_dtheta,
                                              -3 * noise,
                                              noise / n_samples);


        // Eq 39 (with double-negation on term inside parens)
        db_dtheta = centerSample - db_dtheta;
        Matrix<double, 6, 6> scale = P_theta_theta_inv +
                                     (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta;

        // eq 14b, mutatis mutandis (grumble, grumble)
        Matrix<double, 6, 1> increment;
        scale.ldlt().solve(dJ_dtheta, &increment);
        estimate -= increment;
    }

    // Transform the estimated parameters from [c | e] back into [b | d].
    for (size_t i = 0; i < 3; ++i) {
        scale.coeffRef(i) = -1 + sqrt(1 + estimate.coeff(3 + i));
        bias.coeffRef(i)  = estimate.coeff(i) / sqrt(1 + estimate.coeff(3 + i));
    }
}
Ejemplo n.º 25
0
//----------------------------------------------------------------------------------------------------
Vector3f Vector3f::Perpendicular( const Vector3f& A )
{
    Vector3f vec = A;
    return vec.Perpendicular();    
}
Ejemplo n.º 26
0
/**
 * Compute the scale factor and bias associated with a vector observer
 * according to the equation:
 * B_k = (I_{3x3} + D)^{-1} \times (O^T A_k H_k + b + \epsilon_k)
 * where k is the sample index,
 *       B_k is the kth measurement
 *       I_{3x3} is a 3x3 identity matrix
 *       D is a 3x3 symmetric matrix of scale factors
 *       O is the orthogonal alignment matrix
 *       A_k is the attitude at the kth sample
 *       b is the bias in the global reference frame
 *       \epsilon_k is the noise at the kth sample
 *
 * After computing the scale factor and bias matrices, the optimal estimate is
 * given by
 * \tilde{B}_k = (I_{3x3} + D)B_k - b
 *
 * This implementation makes the assumption that \epsilon is a constant white,
 * gaussian noise source that is common to all k.  The misalignment matrix O
 * is not computed.
 *
 * @param bias[out] The computed bias, in the global frame
 * @param scale[out] The computed scale factor matrix, in the sensor frame.
 * @param samples[in] An array of measurement samples.
 * @param n_samples The number of samples in the array.
 * @param referenceField The magnetic field vector at this location.
 * @param noise The one-sigma squared standard deviation of the observed noise
 * in the sensor.
 */
void twostep_bias_scale(Vector3f & bias,
                        Matrix3f & scale,
                        const Vector3f samples[],
                        const size_t n_samples,
                        const Vector3f & referenceField,
                        const float noise)
{
    // Define L_k by eq 51 for k = 1 .. n_samples
    Matrix<double, Dynamic, 9> fullSamples(n_samples, 9);
    // \hbar{L} by eq 52, simplified by observing that the common noise term
    // makes this a simple average.
    Matrix<double, 1, 9> centerSample = Matrix<double, 1, 9>::Zero();
    // Define the sample differences z_k by eq 23 a)
    double sampleDeltaMag[n_samples];
    // The center value \hbar{z}
    double sampleDeltaMagCenter = 0;
    // The squared norm of the reference vector
    double refSquaredNorm = referenceField.squaredNorm();

    for (size_t i = 0; i < n_samples; ++i) {
        fullSamples.row(i) << 2 * samples[i].transpose().cast<double>(),
            -(samples[i][0] * samples[i][0]),
            -(samples[i][1] * samples[i][1]),
            -(samples[i][2] * samples[i][2]),
            -2 * (samples[i][0] * samples[i][1]),
            -2 * (samples[i][0] * samples[i][2]),
            -2 * (samples[i][1] * samples[i][2]);

        centerSample += fullSamples.row(i);

        sampleDeltaMag[i]     = samples[i].squaredNorm() - refSquaredNorm;
        sampleDeltaMagCenter += sampleDeltaMag[i];
    }
    sampleDeltaMagCenter /= n_samples;
    centerSample /= n_samples;

    // Define \tilde{L}_k for k = 0 .. n_samples
    Matrix<double, Dynamic, 9> centeredSamples(n_samples, 9);
    // Define \tilde{z}_k for k = 0 .. n_samples
    double centeredMags[n_samples];
    // Compute the term under the summation of eq 57a
    Matrix<double, 9, 1> estimateSummation = Matrix<double, 9, 1>::Zero();
    for (size_t i = 0; i < n_samples; ++i) {
        centeredSamples.row(i) = fullSamples.row(i) - centerSample;
        centeredMags[i] = sampleDeltaMag[i] - sampleDeltaMagCenter;
        estimateSummation += centeredMags[i] * centeredSamples.row(i).transpose();
    }
    estimateSummation /= noise;

    // By eq 57b
    Matrix<double, 9, 9> P_theta_theta_inv = (1.0f / noise) *
                                             centeredSamples.transpose() * centeredSamples;

#ifdef PRINTF_DEBUGGING
    SelfAdjointEigenSolver<Matrix<double, 9, 9> > eig(P_theta_theta_inv);
    std::cout << "P_theta_theta_inverse: \n" << P_theta_theta_inv << "\n\n";
    std::cout << "P_\\tt^-1 eigenvalues: " << eig.eigenvalues().transpose()
              << "\n";
    std::cout << "P_\\tt^-1 eigenvectors:\n" << eig.eigenvectors() << "\n";
#endif

    // The current value of the estimate.  Initialized to \tilde{\theta}^*
    Matrix<double, 9, 1> estimate;
    // By eq 57a
    P_theta_theta_inv.ldlt().solve(estimateSummation, &estimate);

    // estimate i+1 = estimate_i - Fisher^{-1}(at estimate_i)*gradient(theta)
    // Fisher^{-1} = \tilde{Fisher}^-1 + \hbar{Fisher}^{-1}
    size_t count = 0;
    double eta   = 10000;
    while (count++ < 200 && eta > 1e-8) {
        static bool warned = false;
        if (hasNaN(estimate)) {
            std::cout << "WARNING: found NaN at time " << count << "\n";
            warned = true;
        }
#if 0
        SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate));
        Vector3d S = eig_E.eigenvalues();
        Vector3d W; W << -1 + sqrt(1 + S.coeff(0)),
            -1 + sqrt(1 + S.coeff(1)),
            -1 + sqrt(1 + S.coeff(2));
        scale = (eig_E.eigenvectors() * W.asDiagonal() *
                 eig_E.eigenvectors().transpose()).cast<float>();

        (Matrix3f::Identity() + scale).ldlt().solve(
            estimate.start<3>().cast<float>(), &bias);
        std::cout << "\n\nestimated bias: " << bias.transpose()
                  << "\nestimated scale:\n" << scale;
#endif

        Matrix<double, 1, 9> db_dtheta = dnormb_dtheta(estimate);

        Matrix<double, 9, 1> dJ_dtheta = ::dJ_dtheta(centerSample,
                                                     sampleDeltaMagCenter,
                                                     estimate,
                                                     db_dtheta,
                                                     -3 * noise,
                                                     noise / n_samples);

        // Eq 59, with reused storage on db_dtheta
        db_dtheta = centerSample - db_dtheta;
        Matrix<double, 9, 9> scale = P_theta_theta_inv +
                                     (double(n_samples) / noise) * db_dtheta.transpose() * db_dtheta;

        // eq 14b, mutatis mutandis (grumble, grumble)
        Matrix<double, 9, 1> increment;
        scale.ldlt().solve(dJ_dtheta, &increment);
        estimate -= increment;
        eta = increment.dot(scale * increment);
        std::cout << "eta: " << eta << "\n";
    }
    std::cout << "terminated at eta = " << eta
              << " after " << count << " iterations\n";

    if (!isnan(eta) && !isinf(eta)) {
        // Transform the estimated parameters from [c | E] back into [b | D].
        // See eq 63-65
        SelfAdjointEigenSolver<Matrix3d> eig_E(E_theta(estimate));
        Vector3d S = eig_E.eigenvalues();
        Vector3d W; W << -1 + sqrt(1 + S.coeff(0)),
            -1 + sqrt(1 + S.coeff(1)),
            -1 + sqrt(1 + S.coeff(2));
        scale = (eig_E.eigenvectors() * W.asDiagonal() *
                 eig_E.eigenvectors().transpose()).cast<float>();

        (Matrix3f::Identity() + scale).ldlt().solve(
            estimate.start<3>().cast<float>(), &bias);
    } else {
        // return nonsense data.  The eigensolver can fall ingo
        // an infinite loop otherwise.
        // TODO: Add error code return
        scale = Matrix3f::Ones() * std::numeric_limits<float>::quiet_NaN();
        bias  = Vector3f::Zero();
    }
}
void PointCloudInput::read1()
{
    if(port1.isNew()){
        do {
            port1.read();
        } while(port1.isNew());
    
        bool rgb = false;
        for(size_t i=0; i < pointCloud1.fields.length(); ++i){
            string name = string(pointCloud1.fields[i].name);
            pointCloudField[name].offset = pointCloud1.fields[i].offset;
            pointCloudField[name].type = pointCloud1.fields[i].data_type;
            pointCloudField[name].count = pointCloud1.fields[i].count;
            if(i==5){
                rgb = true;
            }
        }

        int n = pointCloud1.height * pointCloud1.width;
        int m = pointCloud1.data.length() / pointCloud1.point_step;
        int numPoints = std::min(n, m);
        unsigned char* src = (unsigned char*)pointCloud1.data.get_buffer();
        auto tmpPoints = std::make_shared<vector<Vector3f>>();
        std::shared_ptr<Image> tmpImage;
        unsigned char* pixels = nullptr;
        if(rgb){
            tmpImage = std::make_shared<Image>();
            tmpImage->setSize(pointCloud1.width, pointCloud1.height, 3);
            pixels = tmpImage->pixels();
        }
        
        tmpPoints->clear();
        tmpPoints->reserve(numPoints);
        
        PointCloudField& fex = pointCloudField["x"];
        PointCloudField& fey = pointCloudField["y"];
        PointCloudField& fez = pointCloudField["z"];
        PointCloudField& fer = pointCloudField["r"];
        PointCloudField& feg = pointCloudField["g"];
        PointCloudField& feb = pointCloudField["b"];
        
        for(int i=0; i < numPoints; ++i, src+=pointCloud1.point_step){
            Vector3f point;
            point.x() = readFloatPointCloudData(&src[fex.offset], fex, pointCloud1.is_bigendian );
            point.y() = readFloatPointCloudData(&src[fey.offset], fey, pointCloud1.is_bigendian );
            point.z() = readFloatPointCloudData(&src[fez.offset], fez, pointCloud1.is_bigendian );
            tmpPoints->push_back(point);
            
            if(rgb && pixels){
                pixels[0] = src[fer.offset];
                pixels[1] = src[feg.offset];
                pixels[2] = src[feb.offset];
                pixels += 3;
            }
        }
        
        if(!rgb || !pixels){
            tmpImage.reset();
        }

        RangeCameraPtr tmpRangeCamera = rangeCamera;
        callLater([tmpRangeCamera, tmpPoints, tmpImage]() mutable {
                tmpRangeCamera->setPoints(tmpPoints);
                if(tmpImage){
                    tmpRangeCamera->setImage(tmpImage);
                }
                tmpRangeCamera->notifyStateChange();
            });
    }
}
Ejemplo n.º 28
0
void BoundingAxis::_createAxisTicks(const Vector3f& start, const Vector3f& end,
                                    const Vector3f& normal,
                                    const Vector3f& normal2, bool flipTick,
                                    BoundingAxisData& bbAxisData)
{
    size_t currentAxis;
    if (Vector3f(end - start).find_max_index() >
        std::numeric_limits<float>::min())
        currentAxis = Vector3f(end - start).find_max_index();
    else
        currentAxis = Vector3f(end - start).find_min_index();

    const Vector2f range = _computeRange(bbAxisData.volInfo, currentAxis);
    const float rangeLength = range.y() - range.x();

    const float worldSpaceLength = start.distance(end);
    const float dataToWorldScale = worldSpaceLength / rangeLength;
    const float startTickValue = bbAxisData.tickDistance *
                                 std::ceil(range.x() / bbAxisData.tickDistance);
    const float worldSpaceOffset =
        (startTickValue - range.x()) * dataToWorldScale;
    const float worldSpaceTickDistance =
        bbAxisData.tickDistance * dataToWorldScale;

    const int32_t numberOfTick =
        (range.y() - startTickValue) / bbAxisData.tickDistance + 1;
    const Vector3f axisDir = vmml::normalize(end - start);

    Vector3f cross = vmml::normalize(vmml::cross(axisDir, normal));
    size_t tickAxis;
    if (cross.find_max() <= std::numeric_limits<float>::min())
        tickAxis = cross.find_min_index();
    else
        tickAxis = cross.find_max_index();

    for (int32_t i = 0; i < numberOfTick; ++i)
    {
        float tickHeight = 0.014f;
        Vector4f color(1.0, 0.69, 0.69, 1.0);
        const int32_t startTickValueInteger =
            std::round(startTickValue / bbAxisData.factor);
        const int32_t tickDistanceInteger =
            std::round(bbAxisData.tickDistance / bbAxisData.factor);

        if (((startTickValueInteger + tickDistanceInteger * i) %
             (tickDistanceInteger * 5)) == 0)
        {
            tickHeight *= 1.5;
            color = Vector4f(1.0, 1.0, 0.69, 1.0);
        }

        int sign = (0.0f < cross[tickAxis]) - (cross[tickAxis] < 0.0f);
        if (flipTick)
            sign = -sign;

        Vector3f posOnAxis =
            start + axisDir * (worldSpaceOffset + worldSpaceTickDistance * i);
        append3dVector(bbAxisData.vertices, posOnAxis);
        posOnAxis[tickAxis] = posOnAxis[tickAxis] + sign * tickHeight;
        append3dVector(bbAxisData.vertices, posOnAxis);

        for (uint32_t j = 0; j < 2; ++j)
        {
            append3dVector(bbAxisData.normals, normal);
            append3dVector(bbAxisData.normals2, normal2);
            append4dVector(bbAxisData.colors, color);
            bbAxisData.types.push_back(AXIS);
        }

        _nVertices += 2;
    }
}
Ejemplo n.º 29
0
// perform drift correction. This function aims to update _omega_P and
// _omega_I with our best estimate of the short term and long term
// gyro error. The _omega_P value is what pulls our attitude solution
// back towards the reference vector quickly. The _omega_I term is an
// attempt to learn the long term drift rate of the gyros.
//
// This function also updates _omega_yaw_P with a yaw correction term
// from our yaw reference vector
void
AP_AHRS_DCM::drift_correction(float deltat)
{
    float error_course = 0;
    Vector3f accel;
    Vector3f error;
    float error_norm = 0;
    float yaw_deltat = 0;

    accel = _accel_vector;

    // if enabled, use the GPS to correct our accelerometer vector
    // for centripetal forces
    if(_centripetal &&
       _gps != NULL &&
       _gps->status() == GPS::GPS_OK) {
        accel_adjust(accel);
    }


    //*****Roll and Pitch***************

    // normalise the accelerometer vector to a standard length
    // this is important to reduce the impact of noise on the
    // drift correction, as very noisy vectors tend to have
    // abnormally high lengths. By normalising the length we
    // reduce their impact.
    float accel_length = accel.length();

    accel *= (_gravity / accel_length);
    if (accel.is_inf()) {
        // we can't do anything useful with this sample
        _omega_P.zero();
        return;
    }

    // calculate the error, in m/s^2, between the attitude
    // implied by the accelerometers and the attitude
    // in the current DCM matrix
    error =  _dcm_matrix.c % accel;

    // Limit max error to limit the effect of noisy values
    // on the algorithm. This limits the error to about 11
    // degrees
    error_norm = error.length();

    if (error_norm > 2) {
        error *= (2 / error_norm);
    }

    // we now want to calculate _omega_P and _omega_I. The
    // _omega_P value is what drags us quickly to the
    // accelerometer reading.
    _omega_P = error * _kp_roll_pitch;

    // the _omega_I is the long term accumulated gyro
    // error. This determines how much gyro drift we can
    // handle.
    _omega_I_sum += error * (_ki_roll_pitch * deltat);
    _omega_I_sum_time += deltat;

    // these sums support the reporting of the DCM state via MAVLink
    _error_rp_sum += error_norm;
    _error_rp_count++;

    // yaw drift correction

    // we only do yaw drift correction when we get a new yaw
    // reference vector. In between times we rely on the gyros for
    // yaw. Avoiding this calculation on every call to
    // update_DCM() saves a lot of time
    if (_compass && _compass->use_for_yaw()) {
        if (_compass->last_update != _compass_last_update) {
            yaw_deltat = 1.0e-6*(_compass->last_update - _compass_last_update);
            if (_have_initial_yaw && yaw_deltat < 2.0) {
                // Equation 23, Calculating YAW error
                // We make the gyro YAW drift correction based
                // on compass magnetic heading
                error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x);
                _compass_last_update = _compass->last_update;
            } else {
                // this is our first estimate of the yaw,
                // or the compass has come back online after
                // no readings for 2 seconds.
                //
                // construct a DCM matrix based on the current
                // roll/pitch and the compass heading.
                // First ensure the compass heading has been
                // calculated
                _compass->calculate(_dcm_matrix);

                // now construct a new DCM matrix
                _compass->null_offsets_disable();
                _dcm_matrix.from_euler(roll, pitch, _compass->heading);
                _compass->null_offsets_enable();
                _have_initial_yaw = true;
                _compass_last_update = _compass->last_update;
                error_course = 0;
            }
        }
    } else if (_gps && _gps->status() == GPS::GPS_OK) {
        if (_gps->last_fix_time != _gps_last_update) {
            // Use GPS Ground course to correct yaw gyro drift
            if (_gps->ground_speed >= GPS_SPEED_MIN) {
                yaw_deltat = 1.0e-3*(_gps->last_fix_time - _gps_last_update);
                if (_have_initial_yaw && yaw_deltat < 2.0) {
                    float course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
                    float course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
                    // Equation 23, Calculating YAW error
                    error_course = (_dcm_matrix.a.x * course_over_ground_y) - (_dcm_matrix.b.x * course_over_ground_x);
                    _gps_last_update = _gps->last_fix_time;
                } else  {
                    // when we first start moving, set the
                    // DCM matrix to the current
                    // roll/pitch values, but with yaw
                    // from the GPS
                    if (_compass) {
                        _compass->null_offsets_disable();
                    }
                    _dcm_matrix.from_euler(roll, pitch, ToRad(_gps->ground_course));
                    if (_compass) {
                        _compass->null_offsets_enable();
                    }
                    _have_initial_yaw =  true;
                    error_course = 0;
                    _gps_last_update = _gps->last_fix_time;
                }
            } else if (_gps->ground_speed >= GPS_SPEED_RESET) {
                // we are not going fast enough to use GPS for
                // course correction, but we won't reset
                // _have_initial_yaw yet, instead we just let
                // the gyro handle yaw
                error_course = 0;
            } else {
                // we are moving very slowly. Reset
                // _have_initial_yaw and adjust our heading
                // rapidly next time we get a good GPS ground
                // speed
                error_course = 0;
                _have_initial_yaw = false;
            }
        }
    }

    // see if there is any error in our heading relative to the
    // yaw reference. This will be zero most of the time, as we
    // only calculate it when we get new data from the yaw
    // reference source
    if (yaw_deltat == 0 || error_course == 0) {
        // we don't have a new reference heading. Slowly
        // decay the _omega_yaw_P to ensure that if we have
        // lost the yaw reference sensor completely we don't
        // keep using a stale offset
        _omega_yaw_P *= 0.97;
        goto check_sum_time;
    }

    // ensure the course error is scaled from -PI to PI
    if (error_course > PI) {
        error_course -= 2*PI;
    } else if (error_course < -PI) {
        error_course += 2*PI;
    }

    // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft
    // this gives us an error in radians
    error = _dcm_matrix.c * error_course;

    // Adding yaw correction to proportional correction vector. We
    // allow the yaw reference source to affect all 3 components
    // of _omega_yaw_P as we need to be able to correctly hold a
    // heading when roll and pitch are non-zero
    _omega_yaw_P = error * _kp_yaw;

    // add yaw correction to integrator correction vector, but
    // only for the z gyro. We rely on the accelerometers for x
    // and y gyro drift correction. Using the compass or GPS for
    // x/y drift correction is too inaccurate, and can lead to
    // incorrect builups in the x/y drift. We rely on the
    // accelerometers to get the x/y components right
    _omega_I_sum.z += error.z * (_ki_yaw * yaw_deltat);

    // we keep the sum of yaw error for reporting via MAVLink.
    _error_yaw_sum += error_course;
    _error_yaw_count++;

check_sum_time:
    if (_omega_I_sum_time > 10) {
        // every 10 seconds we apply the accumulated
        // _omega_I_sum changes to _omega_I. We do this to
        // prevent short term feedback between the P and I
        // terms of the controller. The _omega_I vector is
        // supposed to refect long term gyro drift, but with
        // high noise it can start to build up due to short
        // term interactions. By summing it over 10 seconds we
        // prevent the short term interactions and can apply
        // the slope limit more accurately
        float drift_limit = _gyro_drift_limit * _omega_I_sum_time;
        _omega_I_sum.x = constrain(_omega_I_sum.x, -drift_limit, drift_limit);
        _omega_I_sum.y = constrain(_omega_I_sum.y, -drift_limit, drift_limit);
        _omega_I_sum.z = constrain(_omega_I_sum.z, -drift_limit, drift_limit);

        _omega_I += _omega_I_sum;

        // zero the sum
        _omega_I_sum.zero();
        _omega_I_sum_time = 0;
    }
}
Ejemplo n.º 30
0
void Renderer::renderSLG() {

    FileUtils::createDir("slg");
    std::string currentDir = FileUtils::getCurrentDir();
    FileUtils::changeCurrentDir("slg");

    std::string imageFilename = tfm::format("../%s-images/frame-%04d.png", _animationName, _frameIndex);

    int width = 1280;
    int height = 720;
    int spp = 64;

    {
        std::ofstream os("render.cfg");
        os << "opencl.platform.index = -1" << std::endl;
        os << "opencl.cpu.use = 0" << std::endl;
        os << "opencl.gpu.use = 1" << std::endl;
        os << "opencl.gpu.workgroup.size = 64" << std::endl;
        os << "scene.epsilon.min = 9.99999971718068536574719e-10" << std::endl;
        os << "scene.epsilon.max = 0.100000001490116119384766" << std::endl;
        os << "renderengine.type = PATHOCL" << std::endl;
        os << "accelerator.instances.enable = 0" << std::endl;
        os << "path.maxdepth = 12" << std::endl;
        os << tfm::format("film.width = %d", width) << std::endl;
        os << tfm::format("film.height = %d", height) << std::endl;
        os << tfm::format("image.filename = %s", imageFilename) << std::endl;
        os << "sampler.type = RANDOM" << std::endl;
        os << "film.filter.type = GAUSSIAN" << std::endl;
        os << "film.filter.xwidth = 1" << std::endl;
        os << "film.filter.ywidth = 1" << std::endl;
        os << "scene.file = scene.scn" << std::endl;
        os << "film.imagepipeline.0.type = TONEMAP_LINEAR" << std::endl;
        os << "film.imagepipeline.1.type = GAMMA_CORRECTION" << std::endl;
        os << "film.imagepipeline.1.value = 2.2" << std::endl;
        os << tfm::format("batch.haltspp = %d", spp) << std::endl;
    }

    {
        std::ofstream os("scene.scn");

        Vector3f origin = _engine.camera().position();
        Vector3f target = _engine.camera().target();
        Vector3f up = _engine.camera().up();
        float fov = _engine.camera().fov() * 1.6f;

        os << tfm::format("scene.camera.lookat.orig = %f %f %f", origin.x(), origin.y(), origin.z()) << std::endl;
        os << tfm::format("scene.camera.lookat.target = %f %f %f", target.x(), target.y(), target.z()) << std::endl;
        os << tfm::format("scene.camera.up = %f %f %f", up.x(), up.y(), up.z()) << std::endl;
        os << tfm::format("scene.camera.fieldofview = %f", fov) << std::endl;

#if 0
#scene.camera.screenwindow = -1 1 -1 1
scene.camera.cliphither = 0.0010000000474974513053894
scene.camera.clipyon = 1.00000001504746621987669e+30
scene.camera.lensradius = 0.00218750000931322574615479
scene.camera.focaldistance = 0.28457677364349365234375
scene.camera.horizontalstereo.enable = 0
scene.camera.horizontalstereo.oculusrift.barrelpostpro.enable = 0
scene.camera.autofocus.enable = 1
#endif

        Vector3f dir = Vector3f(1.f).normalized();
#if 0
        os << tfm::format("scene.sunlight.dir = %f %f %f", dir.x(), dir.y(), dir.z()) << std::endl;
        os << "scene.sunlight.turbidity = 2.2" << std::endl;
        os << "scene.sunlight.relsize = 1" << std::endl;
        os << "scene.sunlight.gain = 0.0001 0.0001 0.0001" << std::endl;
#endif
        os << tfm::format("scene.skylight.dir = %f %f %f", dir.x(), dir.y(), dir.z()) << std::endl;
        os << "scene.skylight.turbidity = 2.2" << std::endl;
        os << "scene.skylight.gain = 0.00007 0.00007 0.00007" << std::endl;

        os << "scene.materials.fluid.type = matte" << std::endl;
        os << "scene.materials.fluid.kd = 0.3 0.3 1.0" << std::endl;
        os << "scene.materials.boundary.type = matte" << std::endl;
        os << "scene.materials.boundary.kd = 0.5 0.5 0.5" << std::endl;
        os << "scene.materials.floor.type = matte" << std::endl;
        os << "scene.materials.floor.kd = 0.2 0.2 0.2" << std::endl;

        // export fluid
        PlyWriter::save(_engine.fluidMesh(), "fluid.ply");
        os << "scene.objects.fluid.material = fluid" << std::endl;
        os << "scene.objects.fluid.ply = fluid.ply" << std::endl;

        // export boundaries
        const auto &boundaryMeshes = _engine.sph().boundaryMeshes();
        for (size_t i = 0; i < boundaryMeshes.size(); ++i) {
            std::string name = tfm::format("boundary-%03d", i);
            std::string filename = name + ".ply";
            PlyWriter::save(boundaryMeshes[i], filename);
            os << tfm::format("scene.objects.%s.material = boundary", name) << std::endl;
            os << tfm::format("scene.objects.%s.ply = %s", name, filename) << std::endl;
        }

        // export floor
        Box3f box = _engine.scene().world.bounds;
        box.max.y() = box.min.y();
        box.min.y() -= 0.01f * box.extents().norm();
        Mesh floorMesh = Mesh::createBox(box);
        PlyWriter::save(floorMesh, "floor.ply");
        os << "scene.objects.floor.material = floor" << std::endl;
        os << "scene.objects.floor.ply = floor.ply" << std::endl;
    }


    std::cout << std::endl;
    std::cout << tfm::format("Rendering frame %d with SLG4 ...", _frameIndex) << std::endl;

    try {
        std::string slg = "/home/freak/luxmark-v3.1/slg4";
        std::string arguments = "-o render.cfg";
        exec_stream_t es;
        // Set 1 minutes timeout
        es.set_wait_timeout(exec_stream_t::s_out | exec_stream_t::s_err | exec_stream_t::s_child, 60*1000);
        es.start(slg, arguments);

        std::string s;
        while (std::getline(es.err(), s).good()) {
            std::cout << s << std::endl;
            if (StringUtils::endsWith(StringUtils::trim(s), "Done.")) {
                DBG("detected finish");
                break;
            }
        }

        DBG("closing slg");
        if (!es.close()) {
            std::cerr << "SLG4 timeout!" << std::endl;
        }
        DBG("closed");
    } catch (const std::exception &e) {
        std::cerr << "exec-stream error: " << e.what() << "\n";
    }

    FileUtils::changeCurrentDir(currentDir);
}