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 Vector3f::Cross( const Vector3f& A, const Vector3f& B )
{    
    return A.Cross( B );
}

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);
//.........这里部分代码省略.........

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; 
//.........这里部分代码省略.........

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;

//.........这里部分代码省略.........

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;
//.........这里部分代码省略.........

// _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
//.........这里部分代码省略.........

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 *