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; }

//---------------------------------------------------------------------------- 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(); } } } }

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); } }

//---------------------------------------------------------------------------------------------------- Vector3f Vector3f::Cross( const Vector3f& A, const Vector3f& B ) { return A.Cross( B ); }

//---------------------------------------------------------------------------------------------------- Vector3f Vector3f::Normalize( const Vector3f& A ) { Vector3f vec = A; vec.Normalize(); return vec; }

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 }

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? }

Vector3f Sphere::getNormal(Vector3f point){ Vector3f N = point - center; return N / N.getLength(); }

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; }

static float getSquaredLength(const Vector3f& v) { return v.dot( v ); }

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)); }

// 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; }

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(); } } }

// _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; }

//============================== // 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 }

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; }

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); }

// 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; }

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); }

//---------------------------------------------------------------------------------------------------- Vector3f Vector3f::Clamp( const Vector3f& A ) { Vector3f vec = A; vec.Clamp(); return vec; }

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; }

//---------------------------------------------------------------------------------------------------- float Vector3f::Dot( const Vector3f& A, const Vector3f& B ) { return A.Dot( B ); }

/** * 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)); } }

//---------------------------------------------------------------------------------------------------- Vector3f Vector3f::Perpendicular( const Vector3f& A ) { Vector3f vec = A; return vec.Perpendicular(); }

/** * 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(); }); } }

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; } }

// 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; } }

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); }