C++ Vec3类代码示例

bool CCameraTracking::Update(SViewParams &viewParams, float &fHOffObstacleStrength, const SCamModeSettings &camMode, const CPlayer &hero, bool bObstacleFound /* = false */)
{
	if(!g_pGameCVars->cl_cam_tracking || !m_pCamRayScan)
		return false;

	m_fFrameTime = max(g_fCamError, gEnv->pTimer->GetFrameTime());

	//in combat mode this function doesn't really avoid obstacles, it avoids clipping
	float fCombatModeWeight = 5.0f;

	//default angle and minimum
	const float fNow = gEnv->pTimer->GetFrameStartTime().GetSeconds();

	CCameraInputHelper *pCamHelper = hero.GetCameraInputHelper();
	CRY_ASSERT(pCamHelper);
	float fLastUserInput = pCamHelper->GetLastUserInputTime();

	//user input overrides auto-follow
	if(fNow - fLastUserInput < 0.5f)
		return false;

	bool bTrackingActive = camMode.camType == ECT_CamFollow && (camMode.collisionType == ECCT_CollisionTrack || camMode.collisionType == ECCT_CollisionTrackOrCut);

	//get current values
	Vec3 curCamDir = viewParams.position-viewParams.targetPos;
	m_curCamOrientation.Set(0.0f, 0.0f, 0.0f);
	CartesianToSpherical(curCamDir, m_curCamOrientation);
	curCamDir.Normalize();

	if(m_curCamOrientation.m_fDist < g_pGameCVars->cl_cam_min_distance)
		m_curCamOrientation.m_fDist = g_pGameCVars->cl_cam_min_distance;

	//work in 0 .. 2PI
	m_curCamOrientation.m_fYaw += gf_PI;

	//if there is something in the way
	if(bObstacleFound)
	{
		//re-start fadeout
		m_fTimeCovered = 0.5f;
		//set last obstacle pos
		m_vLastObstaclePos = viewParams.position;

		//scan obstacle
		if(!IdentifyObstacle(curCamDir, hero))
			return false;
	}
	else if(fabsf(m_fYawDelta) > g_fCamError || fabsf(m_fPitchDelta) > g_fCamError)
	{
		//if there is nothing in the way, fade out the movement

		//time based fade
		if(m_fTimeCovered > 0)
		{
			m_fTimeCovered = max(m_fTimeCovered - m_fFrameTime, 0.0f);

			//these interpolators should be time and not frame based
			m_fYawDelta = (g_fInterpolationRate * m_fYawDelta) * g_fInterpolationWeight;
			m_fPitchDelta = (g_fInterpolationRate * m_fPitchDelta) * g_fInterpolationWeight;
			m_fSpeed = (g_fInterpolationRate * m_fSpeed) * g_fInterpolationWeight;
		}
		else
		{
			m_fYawDelta = 0.0f;
			m_fPitchDelta = 0.0f;
			m_fSpeed = 0.0f;
		}
	}

	//apply delta rotation for obstacle avoidance
	if(fabsf(m_fYawDelta) > g_fCamError || fabsf(m_fPitchDelta) > g_fCamError)
	{
		if(bTrackingActive)
		{
			//set new yaw
			float newYaw = m_curCamOrientation.m_fYaw + m_fYawDelta;
			//re-align yaw
			//the camera direction is 90 degrees off and flipped compared to entity space
			newYaw = (newYaw - gf_PI * 0.5f) * -1.0f;
			//set new pitch
			float newPitch = m_curCamOrientation.m_fPitch + m_fPitchDelta;

			if(g_pGameCVars->cl_cam_orbit != 0)
			{
				//pCamHelper->SetTrackingDelta(-m_fYawDelta, m_fPitchDelta);
				pCamHelper->SetYawDelta(-m_fYawDelta);
				pCamHelper->SetPitchDelta(m_fPitchDelta);
			}
			else
			{
				//apply yaw/pitch on camera
				//pCamHelper->SetInterpolationTarget(newYaw, newPitch, gf_PI, 0.1f, 0.0f);
				//this will always reset follow cam interpolation
				pCamHelper->SetYawDelta(m_fYawDelta);
				pCamHelper->SetPitchDelta(m_fPitchDelta);
			}
		}
		else
		{
			//in orbit mode we basically simulate user input
//.........这里部分代码省略.........

 bool IntersectLineLine(const Vec3<double> & p1, const Vec3<double> & p2, 
                       const Vec3<double> & p3, const Vec3<double> & p4,
                       Vec3<double> & pa, Vec3<double> & pb, 
                       double & mua, double & mub)
 {
     Vec3<double> p13,p43,p21;
     double d1343,d4321,d1321,d4343,d2121;
     double numer,denom;
     
     p13.X() = p1.X() - p3.X();
     p13.Y() = p1.Y() - p3.Y();
     p13.Z() = p1.Z() - p3.Z();
     p43.X() = p4.X() - p3.X();
     p43.Y() = p4.Y() - p3.Y();
     p43.Z() = p4.Z() - p3.Z();
     if (p43.X()==0.0 && p43.Y()==0.0 && p43.Z()==0.0)
         return false;
     p21.X() = p2.X() - p1.X();
     p21.Y() = p2.Y() - p1.Y();
     p21.Z() = p2.Z() - p1.Z();
     if (p21.X()==0.0 && p21.Y()==0.0 && p21.Z()==0.0)
         return false;
     
     d1343 = p13.X() * p43.X() + p13.Y() * p43.Y() + p13.Z() * p43.Z();
     d4321 = p43.X() * p21.X() + p43.Y() * p21.Y() + p43.Z() * p21.Z();
     d1321 = p13.X() * p21.X() + p13.Y() * p21.Y() + p13.Z() * p21.Z();
     d4343 = p43.X() * p43.X() + p43.Y() * p43.Y() + p43.Z() * p43.Z();
     d2121 = p21.X() * p21.X() + p21.Y() * p21.Y() + p21.Z() * p21.Z();
     
     denom = d2121 * d4343 - d4321 * d4321;
     if (denom==0.0)
         return false;
     numer = d1343 * d4321 - d1321 * d4343;
     
     mua = numer / denom;
     mub = (d1343 + d4321 * (mua)) / d4343;
     
     pa.X() = p1.X() + mua * p21.X();
     pa.Y() = p1.Y() + mua * p21.Y();
     pa.Z() = p1.Z() + mua * p21.Z();
     pb.X() = p3.X() + mub * p43.X();
     pb.Y() = p3.Y() + mub * p43.Y();
     pb.Z() = p3.Z() + mub * p43.Z();
     
     return true;
 }

Vec4 RayTracer::traceLights(const Vec3& intersection, Drawable *drawable, Vec3 eye )
{
    Vec4 out_color;
    Vec4 spec_color;
    Vec4 diffuse_color =  drawable->getColorAt(intersection);
    float factor_ambient = drawable->getMaterial().getAmbientFactor();
    LightVector::iterator light = _scene->getLights().begin();
    while (light != _scene->getLights().end()) {
        //Vec3 light_direction = intersection - (*light)->getPosition();
        Vec3 light_direction = (*light)->getPosition() - intersection;
        float lightdist = light_direction.length();
        light_direction.normalize();

        Ray ray(intersection, light_direction);
        float light_scaling = drawable->getNormalAt(intersection) * light_direction;  // no effect if light ray and object normal are orthogonal
        if (light_scaling < 0) 
            light_scaling = 0.0f;

        // check for shadows
        DrawableVector::iterator object = _scene->getDrawables().begin();
        Drawable *hitobject = 0;
        while (object != _scene->getDrawables().end()) {
            float tmp;
            Vec3 tmp_intersection;
            // nothing to do if current object is the drawable
            if ( ((*object) != drawable) && (*object)->intersect(ray, tmp_intersection, tmp) ) {
                // object is between drawable and current light
                Vec3 intersection_vec = tmp_intersection - (*light)->getPosition();
                float dist = intersection_vec.length();
                if ( dist < lightdist) {
                    hitobject = *object;
                    break;
                }

            }
            object++;
        }

        // do specular component
        if ( !hitobject ) {
            switch ((*light)->getType()) {
                case Light::DIRECTIONAL:
                    out_color += (*light)->getColor() * (*light)->getIntensity();
                    out_color += (*light)->getAmbientColor() * factor_ambient;
                    break;
                case Light::POINT:
                    {
                    out_color += (*light)->getColor() * (*light)->getIntensity() * light_scaling * 1.0f/(lightdist);
                    Vec3 half =  light_direction - eye;
                    half.normalize();
                    Vec3 normal = drawable->getNormalAt(intersection);
                    normal.normalize();
                    float shine = 16.0f;
                    float temp = MAX(0.0, normal * half);
                    float spec_val = pow(temp, shine);
                    spec_color += (*light)->getColor() * (*light)->getIntensity() * spec_val * drawable->getMaterial().getReflectionFactor();
                    out_color += (*light)->getAmbientColor() * factor_ambient;
                    break;
                    }
                default:
                    break;
            }
        }

        light++;
    }
    out_color = diffuse_color.componentMultiply(out_color);
    out_color += spec_color;
    return out_color;
}

/** findOutOfRoadSector finds the sector where XYZ is, but as it name
    implies, it is more accurate for the outside of the track than the
    inside, and for STK's needs the accuracy on top of the track is
    unacceptable; but if this was a 2D function, the accuracy for out
    of road sectors would be perfect.

    To find the sector we look for the closest line segment from the
    right and left drivelines, and the number of that segment will be
    the sector.

    The SIDE argument is used to speed up the function only; if we know
    that XYZ is on the left or right side of the track, we know that
    the closest driveline must be the one that matches that condition.
    In reality, the side used in STK is the one from the previous frame,
    but in order to move from one side to another a point would go
    through the middle, that is handled by findRoadSector() which doesn't
    has speed ups based on the side.

    NOTE: This method of finding the sector outside of the road is *not*
    perfect: if two line segments have a similar altitude (but enough to
    let a kart get through) and they are very close on a 2D system,
    if a kart is on the air it could be closer to the top line segment
    even if it is supposed to be on the sector of the lower line segment.
    Probably the best solution would be to construct a quad that reaches
    until the next higher overlapping line segment, and find the closest
    one to XYZ.
 */
int QuadGraph::findOutOfRoadSector(const Vec3& xyz,
                                   const int curr_sector,
                                   std::vector<int> *all_sectors) const
{
    int count = (all_sectors!=NULL) ? (int) all_sectors->size() : getNumNodes();
    int current_sector = 0;
    if(curr_sector != UNKNOWN_SECTOR && !all_sectors)
    {
        // We have to test all nodes here: reason is that on track with
        // shortcuts the n quads of the main drivelines is followed by
        // the quads of the shortcuts. So after quad n-1 (the last one
        // before the lap counting line) quad n will not be 0 (the first
        // quad after the lap counting line), but one of the quads on a
        // shortcut. If we only tested a limited number of quads to
        // improve the performance the crossing of a lap might not be
        // detected (because quad 0 is not tested, only quads on the
        // shortcuts are tested). If this should become a performance
        // bottleneck, we need to set up a graph of 'next' quads for each
        // quad (similar to what the AI does), and only test the quads
        // in this graph.
        const int LIMIT = getNumNodes();
        count           = LIMIT;
        // Start 10 quads before the current quad, so the quads closest
        // to the current position are tested first.
        current_sector  = curr_sector -10;
        if(current_sector<0) current_sector += getNumNodes();
    }

    int   min_sector = UNKNOWN_SECTOR;
    float min_dist_2 = 999999.0f*999999.0f;

    // If a kart is falling and in between (or too far below)
    // a driveline point it might not fulfill
    // the height condition. So we run the test twice: first with height
    // condition, then again without the height condition - just to make sure
    // it always comes back with some kind of quad.
    for(int phase=0; phase<2; phase++)
    {
        for(int j=0; j<count; j++)
        {
            int next_sector;
            if(all_sectors)
                next_sector = (*all_sectors)[j];
            else
                next_sector  = current_sector+1 == (int)getNumNodes()
                ? 0
                : current_sector+1;

            // A first simple test uses the 2d distance to the center of the quad.
            float dist_2 = m_all_nodes[next_sector]->getDistance2FromPoint(xyz);
            if(dist_2<min_dist_2)
            {
                const Quad &q = getQuadOfNode(next_sector);
                float dist    = xyz.getY() - q.getMinHeight();
                // While negative distances are unlikely, we allow some small
                // negative numbers in case that the kart is partly in the
                // track. Only do the height test in phase==0, in phase==1
                // accept any point, independent of height.
                if(phase==1 || (dist < 5.0f && dist>-1.0f) )
                {
                    min_dist_2 = dist_2;
                    min_sector = next_sector;
                }
            }
            current_sector = next_sector;
        }   // for j
        // Leave in phase 0 if any sector was found.
        if(min_sector!=UNKNOWN_SECTOR)
            return min_sector;
    }   // phase

    if(min_sector==UNKNOWN_SECTOR )
    {
//.........这里部分代码省略.........

//------------------------------------------------------------------------
void CVehicleMovementWarrior::Update(const float deltaTime)
{  
  FUNCTION_PROFILER( GetISystem(), PROFILE_GAME );

  if (!IsCollapsing())
    CVehicleMovementHovercraft::Update(deltaTime);
  else
    CVehicleMovementBase::Update(deltaTime);
  
  if (IsCollapsing())
  {
    m_collapseTimer += deltaTime; 

    // check platform
		Vec3 platformPos;

		if (m_pPlatformPos)
			platformPos = m_pPlatformPos->GetWorldTM().GetTranslation();
		else
			platformPos.zero();

    float dist = platformPos.z - GetISystem()->GetI3DEngine()->GetTerrainElevation(platformPos.x, platformPos.y);
    if (dist < 1.f)
    {
      m_platformDown = true;      
    }

    // center turret
    RotatePart(m_pTurret, DEG2RAD(0.f), AXIS_Z, DEG2RAD(2.5f), deltaTime);

    // take down wing and cannon
    RotatePart(m_pWing, DEG2RAD(-12.5f), AXIS_X, DEG2RAD(3.f), deltaTime);
    RotatePart(m_pCannon, DEG2RAD(-20.f), AXIS_X, DEG2RAD(2.5f), deltaTime);

    if (!m_platformDown)
    { 
      // handle legs to bring down platform
      TThrusters::iterator iter;
      for (iter=m_vecThrusters.begin(); iter!=m_vecThrusters.end(); ++iter)
      {
        SThruster* pThruster = *iter;

        if (pThruster->heightAdaption <= 0.f)        
        {
          pThruster->hoverHeight = max(0.1f, pThruster->hoverHeight - 0.6f*deltaTime);
          continue;
        }
        else
        {
          //if (!pThruster->groundContact)          
          //pThruster->hoverHeight = max(0.1f, pThruster->hoverHeight - 0.2f*deltaTime);          
        }

        /* 
        // special legs control
        float collapseSpeed = DEG2RAD(5.f);
        float maxDistMovable = 1.f/0.8f;

        float dist = (isneg(pThruster->prevDist)) ? 0.f : pThruster->hoverHeight - pThruster->prevDist;

        if (isneg(dist))
        {
        collapseSpeed *= max(0.f, 1.f + maxDistMovable*dist);
        }

        if (collapseSpeed > 0.f)
        { 
        float angle = RotatePart(pThruster->pParentPart, DEG2RAD(m_collapsedLegAngle), collapseSpeed, deltaTime);          
        RotatePart(pThruster->pPart, DEG2RAD(m_collapsedFeetAngle), collapseSpeed, deltaTime);
        }
        */
      }      
    }
    else
    {
      if (!m_collapsed)
      {
        Collapsed(true); 
      }
    }
  }
  
  if (IsPowered() && !IsCollapsed())
  { 
    // "normal" legs control here   

    bool bStartComplete = (m_startComplete > 1.5f);
    float adaptionSpeed = IsCollapsing() ? 0.8f : 1.5f;
    int t = 0;

    for (TThrusters::iterator iter=m_vecThrusters.begin(); iter!=m_vecThrusters.end(); ++iter)
    {
      SThruster* pThruster = *iter;
      ++t;

      if (pThruster->heightAdaption > 0.f && bStartComplete && pThruster->pPart && pThruster->pParentPart)
      {         
        const char* footName = pThruster->pPart->GetName().c_str();        
        EWarriorMovement mode = eWM_Hovering;
//.........这里部分代码省略.........

bool CIntersectionAssistanceUnit::GetHighestScoringLastKnownGoodPosition( const QuatT& baseOrientation, QuatT& outQuat ) const
{
	bool bFlippedIsBest = false;

	if(!m_lastKnownGoodPositions.empty())
	{
		// Higher is better
		float fBestScore = 0.0f; 
		int bestIndex = -1; 
		Vec3 vBaseUpDir = baseOrientation.q.GetColumn2().GetNormalized(); 
		for(uint8 i = 0; i < m_lastKnownGoodPositions.size(); ++i)
		{
			const QuatT& qLastKnownGood = m_lastKnownGoodPositions[i]; 
			if(IsPositionWithinAcceptedLimits(qLastKnownGood.t, baseOrientation.t, kDistanceTolerance))
			{
				// Generate [0.0f,1.0f] score for distance
				const Vec3 distVec = (qLastKnownGood.t - baseOrientation.t); 

				const float length = max(distVec.GetLengthFast(),0.0001f);
				const float distanceScore = max(1.0f - (length * kInverseDistanceTolerance) * kDistanceWeight, 0.0f);

				Vec3 vUpDir		 = qLastKnownGood.q.GetColumn2();

				const float regularOrientationScore = vBaseUpDir.Dot(vUpDir);
				const float flippedOrientationScore = vBaseUpDir.Dot(-vUpDir);

				float orientationScore = max(regularOrientationScore, flippedOrientationScore);
				orientationScore *= kOrientationWeight;

				const float fCandidateScore = distanceScore + orientationScore;

#ifndef _RELEASE
				if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 2)
				{
					CryWatch("[INDEX(%d)] : D[%.3f] O[%.3f] T[%.3f] (%s)", i, distanceScore, orientationScore, fCandidateScore, flippedOrientationScore > regularOrientationScore ? "*F*" : "R");
				}
#endif //#ifndef _RELEASE

				if(fCandidateScore > fBestScore)
				{
					bestIndex	 = i; 
					fBestScore = fCandidateScore;
					bFlippedIsBest = (flippedOrientationScore > regularOrientationScore);
				}
			}
		}

		if(bestIndex >= 0)
		{
			outQuat = m_lastKnownGoodPositions[bestIndex];
			if(bFlippedIsBest)
			{
				Matrix34 wMat(outQuat); 
				Vec3 vFlippedUpDir = -outQuat.q.GetColumn2().GetNormalized();
				Vec3 vForwardDir	 = outQuat.q.GetColumn1().GetNormalized(); 
				Vec3 vSideDir			 = -outQuat.q.GetColumn0().GetNormalized();
				Matrix34 wFlippedMat;
				wFlippedMat = Matrix34::CreateFromVectors(vSideDir, vForwardDir, vFlippedUpDir, wMat.GetTranslation());
				outQuat = QuatT(wFlippedMat);

				// Adjust pos (rotating around OOBB centre effectively)
				const IEntity* pSubjectEntity = gEnv->pEntitySystem->GetEntity(m_subjectEntityId);
				if(pSubjectEntity)
				{
					AABB entAABB;
					OBB  entOBB;
					pSubjectEntity->GetLocalBounds(entAABB);
					entOBB.SetOBBfromAABB(Quat(IDENTITY), entAABB);

					Vec3 Centre = wMat.TransformPoint(entOBB.c);
					Vec3 toCentre = Centre - outQuat.t;
					outQuat.t += (toCentre * 2.0f);
				}
			}

#ifndef _RELEASE
			if(g_pGameCVars->pl_pickAndThrow.intersectionAssistDebugEnabled == 2)
			{
				m_currentBestIndex = bestIndex;
				CryWatch("[BEST INDEX] : %d", bestIndex);
			}
#endif // ifndef _RELEASE

			return true;
		}
	}

#ifndef _RELEASE
	m_currentBestIndex = -1;
#endif // ifndef _RELEASE

	return false;
}

void ElasticFoundationForceImpl::processContact
   (const State& state, 
    ContactSurfaceIndex meshIndex, ContactSurfaceIndex otherBodyIndex, 
    const Parameters& param, const std::set<int>& insideFaces,
    Real areaScale, Vector_<SpatialVec>& bodyForces, Real& pe) const 
{
    const ContactGeometry& otherObject = subsystem.getBodyGeometry(set, otherBodyIndex);
    const MobilizedBody& body1 = subsystem.getBody(set, meshIndex);
    const MobilizedBody& body2 = subsystem.getBody(set, otherBodyIndex);
    const Transform t1g = body1.getBodyTransform(state)*subsystem.getBodyTransform(set, meshIndex); // mesh to ground
    const Transform t2g = body2.getBodyTransform(state)*subsystem.getBodyTransform(set, otherBodyIndex); // other object to ground
    const Transform t12 = ~t2g*t1g; // mesh to other object

    // Loop over all the springs, and evaluate the force from each one.

    for (std::set<int>::const_iterator iter = insideFaces.begin(); 
                                       iter != insideFaces.end(); ++iter) {
        int face = *iter;
        UnitVec3 normal;
        bool inside;
        Vec3 nearestPoint = otherObject.findNearestPoint(t12*param.springPosition[face], inside, normal);
        if (!inside)
            continue;
        
        // Find how much the spring is displaced.
        
        nearestPoint = t2g*nearestPoint;
        const Vec3 springPosInGround = t1g*param.springPosition[face];
        const Vec3 displacement = nearestPoint-springPosInGround;
        const Real distance = displacement.norm();
        if (distance == 0.0)
            continue;
        const Vec3 forceDir = displacement/distance;
        
        // Calculate the relative velocity of the two bodies at the contact point.
        
        const Vec3 station1 = body1.findStationAtGroundPoint(state, nearestPoint);
        const Vec3 station2 = body2.findStationAtGroundPoint(state, nearestPoint);
        const Vec3 v1 = body1.findStationVelocityInGround(state, station1);
        const Vec3 v2 = body2.findStationVelocityInGround(state, station2);
        const Vec3 v = v2-v1;
        const Real vnormal = dot(v, forceDir);
        const Vec3 vtangent = v-vnormal*forceDir;
        
        // Calculate the damping force.
        
        const Real area = areaScale * param.springArea[face];
        const Real f = param.stiffness*area*distance*(1+param.dissipation*vnormal);
        Vec3 force = (f > 0 ? f*forceDir : Vec3(0));
        
        // Calculate the friction force.
        
        const Real vslip = vtangent.norm();
        if (f > 0 && vslip != 0) {
            const Real vrel = vslip/transitionVelocity;
            const Real ffriction = 
                f*(std::min(vrel, Real(1))
                 *(param.dynamicFriction+2*(param.staticFriction-param.dynamicFriction)
                 /(1+vrel*vrel))+param.viscousFriction*vslip);
            force += ffriction*vtangent/vslip;
        }

        body1.applyForceToBodyPoint(state, station1, force, bodyForces);
        body2.applyForceToBodyPoint(state, station2, -force, bodyForces);
        pe += param.stiffness*area*displacement.normSqr()/2;
    }
}

void testEwaldPME(bool includeExceptions) {

//      Use amorphous NaCl system for the tests

    const int numParticles = 894;
    const double cutoff = 1.2;
    const double boxSize = 3.00646;
    double tol = 1e-5;

    ReferencePlatform reference;
    System system;
    NonbondedForce* nonbonded = new NonbondedForce();
    nonbonded->setNonbondedMethod(NonbondedForce::Ewald);
    nonbonded->setCutoffDistance(cutoff);
    nonbonded->setEwaldErrorTolerance(tol);

    for (int i = 0; i < numParticles/2; i++)
        system.addParticle(22.99);
    for (int i = 0; i < numParticles/2; i++)
        system.addParticle(35.45);
    for (int i = 0; i < numParticles/2; i++)
        nonbonded->addParticle(1.0, 1.0,0.0);
    for (int i = 0; i < numParticles/2; i++)
        nonbonded->addParticle(-1.0, 1.0,0.0);
    system.setDefaultPeriodicBoxVectors(Vec3(boxSize, 0, 0), Vec3(0, boxSize, 0), Vec3(0, 0, boxSize));
    system.addForce(nonbonded);

    vector<Vec3> positions(numParticles);
    #include "nacl_amorph.dat"
    if (includeExceptions) {
        // Add some exclusions.

        for (int i = 0; i < numParticles-1; i++) {
            Vec3 delta = positions[i]-positions[i+1];
            if (sqrt(delta.dot(delta)) < 0.5*cutoff)
                nonbonded->addException(i, i+1, i%2 == 0 ? 0.0 : 0.5, 1.0, 0.0);
        }
    }

//    (1)  Check whether the Reference and CPU platforms agree when using Ewald Method

    VerletIntegrator integrator1(0.01);
    VerletIntegrator integrator2(0.01);
    Context cpuContext(system, integrator1, platform);
    Context referenceContext(system, integrator2, reference);
    cpuContext.setPositions(positions);
    referenceContext.setPositions(positions);
    State cpuState = cpuContext.getState(State::Forces | State::Energy);
    State referenceState = referenceContext.getState(State::Forces | State::Energy);
    tol = 1e-2;
    for (int i = 0; i < numParticles; i++) {
        ASSERT_EQUAL_VEC(referenceState.getForces()[i], cpuState.getForces()[i], tol);
    }
    tol = 1e-5;
    ASSERT_EQUAL_TOL(referenceState.getPotentialEnergy(), cpuState.getPotentialEnergy(), tol);

//    (2) Check whether Ewald method in CPU is self-consistent

    double norm = 0.0;
    for (int i = 0; i < numParticles; ++i) {
        Vec3 f = cpuState.getForces()[i];
        norm += f[0]*f[0] + f[1]*f[1] + f[2]*f[2];
    }

    norm = std::sqrt(norm);
    const double delta = 5e-3;
    double step = delta/norm;
    for (int i = 0; i < numParticles; ++i) {
        Vec3 p = positions[i];
        Vec3 f = cpuState.getForces()[i];
        positions[i] = Vec3(p[0]-f[0]*step, p[1]-f[1]*step, p[2]-f[2]*step);
    }
    VerletIntegrator integrator3(0.01);
    Context cpuContext2(system, integrator3, platform);
    cpuContext2.setPositions(positions);

    tol = 1e-2;
    State cpuState2 = cpuContext2.getState(State::Energy);
    ASSERT_EQUAL_TOL(norm, (cpuState2.getPotentialEnergy()-cpuState.getPotentialEnergy())/delta, tol)

//    (3)  Check whether the Reference and CPU platforms agree when using PME

    nonbonded->setNonbondedMethod(NonbondedForce::PME);
    cpuContext.reinitialize();
    referenceContext.reinitialize();
    cpuContext.setPositions(positions);
    referenceContext.setPositions(positions);
    cpuState = cpuContext.getState(State::Forces | State::Energy);
    referenceState = referenceContext.getState(State::Forces | State::Energy);
    tol = 1e-2;
    for (int i = 0; i < numParticles; i++) {
        ASSERT_EQUAL_VEC(referenceState.getForces()[i], cpuState.getForces()[i], tol);
    }
    tol = 1e-5;
    ASSERT_EQUAL_TOL(referenceState.getPotentialEnergy(), cpuState.getPotentialEnergy(), tol);

//    (4) Check whether PME method in CPU is self-consistent

    norm = 0.0;
    for (int i = 0; i < numParticles; ++i) {
//.........这里部分代码省略.........

//-------------------------------------------
void CGameRules::ProcessClientExplosionScreenFX(const ExplosionInfo &explosionInfo)
{
	IActor *pClientActor = g_pGame->GetIGameFramework()->GetClientActor();
	if (pClientActor)
	{
		//Distance
		float dist = (pClientActor->GetEntity()->GetWorldPos() - explosionInfo.pos).len();

		//Is the explosion in Player's FOV (let's suppose the FOV a bit higher, like 80)
		CActor *pActor = (CActor *)pClientActor;
		SMovementState state;
		if (IMovementController *pMV = pActor->GetMovementController())
		{
			pMV->GetMovementState(state);
		}

		Vec3 eyeToExplosion = explosionInfo.pos - state.eyePosition;
		eyeToExplosion.Normalize();
		bool inFOV = (state.eyeDirection.Dot(eyeToExplosion) > 0.68f);
		
		// if in a vehicle eyeDirection is wrong
		if(pActor && pActor->GetLinkedVehicle())
		{
			Vec3 eyeDir = static_cast<CPlayer*>(pActor)->GetVehicleViewDir();
			inFOV = (eyeDir.Dot(eyeToExplosion) > 0.68f);
		}

		//All explosions have radial blur (default 30m radius, to make Sean happy =))
		float maxBlurDistance = (explosionInfo.maxblurdistance>0.0f)?explosionInfo.maxblurdistance:30.0f;
		if (maxBlurDistance>0.0f && g_pGameCVars->g_radialBlur>0.0f && m_explosionScreenFX && explosionInfo.radius>0.5f)
		{		
			if (inFOV && dist < maxBlurDistance)
			{
				ray_hit hit;
				int col = gEnv->pPhysicalWorld->RayWorldIntersection(explosionInfo.pos , -eyeToExplosion*dist, ent_static | ent_terrain, rwi_stop_at_pierceable|rwi_colltype_any, &hit, 1);

				//If there was no obstacle between flashbang grenade and player
				if(!col)
				{
					float blurRadius = (-1.0f/maxBlurDistance)*dist + 1.0f;

					gEnv->p3DEngine->SetPostEffectParam("FilterRadialBlurring_Radius", blurRadius);
					gEnv->p3DEngine->SetPostEffectParam("FilterRadialBlurring_Amount", 1.0f);

					//CActor *pActor = (CActor *)pClientActor;
					if (pActor->GetScreenEffects() != 0)
					{
						CPostProcessEffect *pBlur = new CPostProcessEffect(pClientActor->GetEntityId(),"FilterRadialBlurring_Amount", 0.0f);
						CLinearBlend *pLinear = new CLinearBlend(1.0f);
						pActor->GetScreenEffects()->StartBlend(pBlur, pLinear, 1.0f, 98);
						pActor->GetScreenEffects()->SetUpdateCoords("FilterRadialBlurring_ScreenPosX","FilterRadialBlurring_ScreenPosY", explosionInfo.pos);
					}

					float distAmp = 1.0f - (dist / maxBlurDistance);
					if (gEnv->pInput) gEnv->pInput->ForceFeedbackEvent( SFFOutputEvent(eDI_XI, eFF_Rumble_Basic, 0.5f, distAmp*3.0f, 0.0f));
				}
			}
		}

		//Flashbang effect 
		if(dist<explosionInfo.radius && inFOV &&
			(!strcmp(explosionInfo.effect_class,"flashbang") || !strcmp(explosionInfo.effect_class,"FlashbangAI")))
		{
			ray_hit hit;
			int col = gEnv->pPhysicalWorld->RayWorldIntersection(explosionInfo.pos , -eyeToExplosion*dist, ent_static | ent_terrain, rwi_stop_at_pierceable|rwi_colltype_any, &hit, 1);

			//If there was no obstacle between flashbang grenade and player
			if(!col)
			{
				float power = explosionInfo.flashbangScale;
				power *= max(0.0f, 1 - (dist/explosionInfo.radius));
				float lookingAt = (eyeToExplosion.Dot(state.eyeDirection.normalize()) + 1)*0.5f;
				power *= lookingAt;

				SAFE_HUD_FUNC(OnFlashbang(1.0f + (power * 4), explosionInfo.blindAmount));

				SAFE_SOUNDMOODS_FUNC(AddSoundMood(SOUNDMOOD_EXPLOSION,MIN(power*40.0f,100.0f)));

				gEnv->p3DEngine->SetPostEffectParam("Flashbang_Time", 1.0f + (power * 4));
				gEnv->p3DEngine->SetPostEffectParam("FlashBang_BlindAmount",explosionInfo.blindAmount);
				gEnv->p3DEngine->SetPostEffectParam("Flashbang_DifractionAmount", (power * 2));
				gEnv->p3DEngine->SetPostEffectParam("Flashbang_Active", 1);
			}
		}
		else if(inFOV && (dist < explosionInfo.radius))
		{
			if (explosionInfo.damage>10.0f || explosionInfo.pressure>100.0f)
			{
				//Add some angular impulse to the client actor depending on distance, direction...
				float dt = (1.0f - dist/explosionInfo.radius);
				dt = dt * dt;
				float angleZ = g_PI*0.15f*dt;
				float angleX = g_PI*0.15f*dt;

				pActor->AddAngularImpulse(Ang3(Random(-angleX*0.5f,angleX),0.0f,Random(-angleZ,angleZ)),0.0f,dt*2.0f);
			}
		}


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

void
SoundD3D::Localize()
{
#ifdef DIRECT_SOUND_3D
	if (sound3d) {
		sound3d->SetMinDistance(min_dist,                        DS3D_IMMEDIATE);
		sound3d->SetMaxDistance(max_dist,                        DS3D_IMMEDIATE);
		sound3d->SetPosition(location.x, location.y, location.z, DS3D_IMMEDIATE);
		sound3d->SetVelocity(velocity.x, velocity.y, velocity.z, DS3D_IMMEDIATE);
	}

#else

	// if no buffer, nothing to do:
	if (!buffer) {
		moved = false;
		return;
	}

	// Compute pan and volume from scratch:
	
	if ((flags & LOC_3D) && creator) {
		Vec3 loc = location;

		SoundCardD3D* ears = (SoundCardD3D*) creator;
		Camera& listener = ears->listener;
		Vec3  ear_loc = listener.Pos();  ear_loc.SwapYZ();
		Vec3  direction = loc - ear_loc;
		
		loc.x = direction * listener.vrt();
		loc.y = direction * listener.vup();
		loc.z = direction * listener.vpn();

		double pan = 10000;
		if (loc.z != 0.0f) pan = fabs(1000.0f * loc.x / loc.z);
		if (pan > 10000)   pan = 10000;
		if (loc.x < 0)     pan = -pan;

		if (volume > 0)
		volume = 0;

		double vol   = volume;
		double mind2 = min_dist * min_dist;
		double maxd2 = max_dist * max_dist;
		double d2    = (loc.x*loc.x) + (loc.y*loc.y) + (loc.z*loc.z);

		if (d2 > maxd2)
		vol = -10000;
		else if (d2 > mind2)
		vol -= (d2-mind2)/(maxd2-mind2) * (vol+10000);

		// clamp volume to legal range:
		if (vol < -10000) vol = -10000;
		else if (vol > volume) vol = volume;

		/***
	Print("Localize: ears  = (%f, %f, %f)\n", ear_loc.x, ear_loc.y, ear_loc.z);
	Print("          world = (%f, %f, %f)\n", location.x, location.y, location.z);
	Print("          view  = (%f, %f, %f)\n", loc.x, loc.y, loc.z);
	Print("          Pan=%f  Volume=%f\n", pan, vol);
	/***/

		HRESULT hr = buffer->SetPan((LONG) pan);
		if (!SUCCEEDED(hr)) {
			char warn[512];
			sprintf_s(warn, "Warning could not set pan on buffer to %f", pan);
			SoundD3DError(warn, hr);
		}

		hr = buffer->SetVolume((LONG) vol);
		if (!SUCCEEDED(hr)) {
			char warn[512];
			sprintf_s(warn, "Warning: could not set volume on buffer to %f", vol);
			SoundD3DError(warn, hr);
		}
		
		// if not too far to hear...
		if ((flags & DOPPLER) && (d2 < maxd2)) {
			// COMPUTE DOPPLER SHIFT:
			const float c = 10000.0f;
			
			direction.Normalize();
			float v_L = ears->velocity * direction;
			float v_S =       velocity * direction;
			
			DWORD f_shift = wfex.nSamplesPerSec;

			if (v_L != v_S) {
				// towards listener:
				if (v_S < 0)
				f_shift = wfex.nSamplesPerSec + 20;
				else
				f_shift = wfex.nSamplesPerSec - 20;
			}

			// distance rolloff of high frequencies:
			double dist     = sqrt(d2);
			DWORD  roll_off = (DWORD) (80 * dist/max_dist);

			f_shift -= roll_off;
//.........这里部分代码省略.........

void testErrorTolerance(NonbondedForce::NonbondedMethod method) {
    // Create a cloud of random point charges.

    const int numParticles = 51;
    const double boxWidth = 5.0;
    System system;
    system.setDefaultPeriodicBoxVectors(Vec3(boxWidth, 0, 0), Vec3(0, boxWidth, 0), Vec3(0, 0, boxWidth));
    NonbondedForce* force = new NonbondedForce();
    system.addForce(force);
    vector<Vec3> positions(numParticles);
    OpenMM_SFMT::SFMT sfmt;
    init_gen_rand(0, sfmt);

    for (int i = 0; i < numParticles; i++) {
        system.addParticle(1.0);
        force->addParticle(-1.0+i*2.0/(numParticles-1), 1.0, 0.0);
        positions[i] = Vec3(boxWidth*genrand_real2(sfmt), boxWidth*genrand_real2(sfmt), boxWidth*genrand_real2(sfmt));
    }
    force->setNonbondedMethod(method);

    // For various values of the cutoff and error tolerance, see if the actual error is reasonable.

    for (double cutoff = 1.0; cutoff < boxWidth/2; cutoff *= 1.2) {
        force->setCutoffDistance(cutoff);
        vector<Vec3> refForces;
        double norm = 0.0;
        for (double tol = 5e-5; tol < 1e-3; tol *= 2.0) {
            force->setEwaldErrorTolerance(tol);
            VerletIntegrator integrator(0.01);
            Context context(system, integrator, platform);
            context.setPositions(positions);
            State state = context.getState(State::Forces);
            if (refForces.size() == 0) {
                refForces = state.getForces();
                for (int i = 0; i < numParticles; i++)
                    norm += refForces[i].dot(refForces[i]);
                norm = sqrt(norm);
            }
            else {
                double diff = 0.0;
                for (int i = 0; i < numParticles; i++) {
                    Vec3 delta = refForces[i]-state.getForces()[i];
                    diff += delta.dot(delta);
                }
                diff = sqrt(diff)/norm;
                ASSERT(diff < 2*tol);
            }
            if (method == NonbondedForce::PME) {
                // See if the PME parameters were calculated correctly.

                double expectedAlpha, actualAlpha;
                int expectedSize[3], actualSize[3];
                NonbondedForceImpl::calcPMEParameters(system, *force, expectedAlpha, expectedSize[0], expectedSize[1], expectedSize[2]);
                force->getPMEParametersInContext(context, actualAlpha, actualSize[0], actualSize[1], actualSize[2]);
                ASSERT_EQUAL_TOL(expectedAlpha, actualAlpha, 1e-5);
                for (int i = 0; i < 3; i++) {
                    ASSERT(actualSize[i] >= expectedSize[i]);
                    ASSERT(actualSize[i] < expectedSize[i]+10);
                }
            }
        }
    }
}

Mat4::Mat4(Vec3 a, double r)
{
	// Create a rotation matrix for an arbitrary axis.
	// See Wikipedia or whatever for the formulae.

	double cosR = cos(r);
	double sinR = sin(r);

	m[0] = cosR + a.x() * a.x() * (1 - cosR);
	m[1] = a.y() * a.x() * (1 - cosR) + a.z() * sinR;
	m[2] = a.z() * a.x() * (1 - cosR) - a.y() * sinR;
	m[3] = 0;

	m[4] = a.x() * a.y() * (1 - cosR) - a.z() * sinR;
	m[5] = cosR + a.y() * a.y() * (1 - cosR);
	m[6] = a.z() * a.y() * (1 - cosR) + a.x() * sinR;
	m[7] = 0;

	m[8]  = a.x() * a.z() * (1 - cosR) + a.y() * sinR;
	m[9]  = a.y() * a.z() * (1 - cosR) - a.x() * sinR;
	m[10] = cosR + a.z() * a.z() * (1 - cosR);
	m[11] = 0;

	m[12] = 0;
	m[13] = 0;
	m[14] = 0;
	m[15] = 1;
}

// Dot product
double Vec3::dot(Vec3& o)
{
	return x() * o.x()  +  y() * o.y()  +  z() * o.z();
}

/** Actually reads in the data from the xml file.
 *  \param root Root of the xml tree.
 */
void KartProperties::getAllData(const XMLNode * root)
{
    root->get("version",           &m_version);

    root->get("name",              &m_name             );

    root->get("icon-file",         &m_icon_file        );

    root->get("minimap-icon-file", &m_minimap_icon_file);

    root->get("shadow-file",       &m_shadow_file      );
    Vec3 c;
    root->get("rgb",               &c                  );
    m_color.set(255, (int)(255*c.getX()), (int)(255*c.getY()), (int)(255*c.getZ()));

    root->get("groups",            &m_groups           );

    root->get("random-wheel-rot",  &m_has_rand_wheels  );

    root->get("shadow-scale",      &m_shadow_scale     );
    root->get("shadow-x-offset",   &m_shadow_x_offset  );
    root->get("shadow-y-offset",   &m_shadow_y_offset  );

    root->get("type",     &m_kart_type        );

    if(const XMLNode *dimensions_node = root->getNode("center"))
        dimensions_node->get("gravity-shift", &m_gravity_center_shift);

    if(const XMLNode *ai_node = root->getNode("ai"))
    {
        const XMLNode *easy = ai_node->getNode("easy");
        m_ai_properties[RaceManager::DIFFICULTY_EASY]->load(easy);
        const XMLNode *medium = ai_node->getNode("medium");
        m_ai_properties[RaceManager::DIFFICULTY_MEDIUM]->load(medium);
        const XMLNode *hard = ai_node->getNode("hard");
        m_ai_properties[RaceManager::DIFFICULTY_HARD]->load(hard);
        const XMLNode *best = ai_node->getNode("best");
        m_ai_properties[RaceManager::DIFFICULTY_BEST]->load(best);
    }

    if(const XMLNode *suspension_node = root->getNode("suspension"))
    {
        suspension_node->get("stiffness",            &m_suspension_stiffness);
        suspension_node->get("rest",                 &m_suspension_rest     );
        suspension_node->get("travel-cm",            &m_suspension_travel_cm);
        suspension_node->get("exp-spring-response",  &m_exp_spring_response );
        suspension_node->get("max-force",            &m_max_suspension_force);
    }

    if(const XMLNode *wheels_node = root->getNode("wheels"))
    {
        wheels_node->get("damping-relaxation",  &m_wheel_damping_relaxation );
        wheels_node->get("damping-compression", &m_wheel_damping_compression);
        wheels_node->get("radius",              &m_wheel_radius             );
    }

    if(const XMLNode *speed_weighted_objects_node = root->getNode("speed-weighted-objects"))
    {
        m_speed_weighted_object_properties.loadFromXMLNode(speed_weighted_objects_node);
    }

    if(const XMLNode *friction_node = root->getNode("friction"))
        friction_node->get("slip", &m_friction_slip);

    if(const XMLNode *stability_node = root->getNode("stability"))
    {
        stability_node->get("roll-influence",
                                                   &m_roll_influence         );
        stability_node->get("chassis-linear-damping",
                                                   &m_chassis_linear_damping );
        stability_node->get("chassis-angular-damping",
                                                   &m_chassis_angular_damping);
        stability_node->get("downward-impulse-factor",
                                                   &m_downward_impulse_factor);
        stability_node->get("track-connection-accel",
                                                   &m_track_connection_accel );
    }

    if(const XMLNode *collision_node = root->getNode("collision"))
    {
        collision_node->get("impulse",         &m_collision_impulse        );
        collision_node->get("impulse-time",    &m_collision_impulse_time   );
        collision_node->get("terrain-impulse", &m_collision_terrain_impulse);
        collision_node->get("restitution",     &m_restitution              );
        collision_node->get("bevel-factor",    &m_bevel_factor             );
        std::string s;
        collision_node->get("impulse-type",    &s                          );
        s = StringUtils::toLowerCase(s);
        if(s=="none")
            m_terrain_impulse_type = IMPULSE_NONE;
        else if(s=="normal")
            m_terrain_impulse_type = IMPULSE_NORMAL;
        else if(s=="driveline")
            m_terrain_impulse_type = IMPULSE_TO_DRIVELINE;
        else
        {
            Log::fatal("[KartProperties]",
//.........这里部分代码省略.........

//.........这里部分代码省略.........
	//	return;
	//}

	bool bTranslating = false;
	Vec4 atWorld(0,0,0,0);
	Vec4 rightWorld(0,0,0,0);
	Vec4 upWorld(0,0,0,0);

	if (m_bKey['W'] || m_bKey['S'])
	{
		// In D3D, the "look at" default is always
		// the positive Z axis.
		Vec4 at = g_Forward4; 
		if (m_bKey['S'])
			at *= -1;

		// This will give us the "look at" vector 
		// in world space - we'll use that to move
		// the camera.
		atWorld = m_matToWorld.Xform(at);
		bTranslating = true;
	}

	if (m_bKey['A'] || m_bKey['D'])
	{
		// In D3D, the "right" default is always
		// the positive X axis.
		Vec4 right = g_Right4; 
		if (m_bKey['A'])
			right *= -1;

		// This will give us the "right" vector 
		// in world space - we'll use that to move
		// the camera
		rightWorld = m_matToWorld.Xform(right);
		bTranslating = true;
	}

	if (m_bKey[' '] || m_bKey['C'] || m_bKey['X'])
	{
		// In D3D, the "up" default is always
		// the positive Y axis.
		Vec4 up = g_Right4; 
		if (!m_bKey[' '])
			up *= -1;

		//Unlike strafing, Up is always up no matter
		//which way you are looking
		upWorld = up;
		bTranslating = true;
	}

	//Handling rotation as a result of mouse position
	{
		// The secret formula!!! Don't give it away!
		//If you are seeing this now, then you must be some kind of elite hacker!
		m_fYaw += (m_fTargetYaw - m_fYaw) * ( .35f );
		m_fTargetPitch = MAX(-90, MIN(90, m_fTargetPitch));
		m_fPitch += (m_fTargetPitch - m_fPitch) * ( .35f );

		// Calculate the new rotation matrix from the camera
		// yaw and pitch.
		Mat4x4 matRot;
		matRot.BuildYawPitchRoll(DEGREES_TO_RADIANS(-m_fYaw), DEGREES_TO_RADIANS(m_fPitch), 0);

		// Create the new object-to-world matrix, and the
		// new world-to-object matrix. 

		m_matToWorld = matRot * m_matPosition;
		m_matFromWorld = m_matToWorld.Inverse(); 
		m_object->VSetTransform(&m_matToWorld, &m_matFromWorld);
	}

	if (bTranslating)
	{
		float elapsedTime = (float)deltaMilliseconds / 1000.0f;

		Vec3 direction = atWorld + rightWorld + upWorld;
		direction.Normalize(); 

		// Ramp the acceleration by the elapsed time.
		float numberOfSeconds = 5.f;
		m_currentSpeed += m_maxSpeed * ( (elapsedTime*elapsedTime) / numberOfSeconds);
		if (m_currentSpeed > m_maxSpeed)
			m_currentSpeed = m_maxSpeed;

		direction *= m_currentSpeed;

		Vec3 pos = m_matPosition.GetPosition() + direction;
		m_matPosition.SetPosition(pos);
		m_matToWorld.SetPosition(pos);

		m_matFromWorld = m_matToWorld.Inverse();
		m_object->VSetTransform(&m_matToWorld, &m_matFromWorld);
	}
	else
	{
		m_currentSpeed = 0.0f;
	}
}