自学教程：C++ Abs函数代码示例

void CBuzzControllerEFootBot::SetWheelSpeedsFromVector(const CVector2& c_heading) {   /* Get the heading angle */   CRadians cHeadingAngle = c_heading.Angle().SignedNormalize();   /* Get the length of the heading vector */   Real fHeadingLength = c_heading.Length();   /* Clamp the speed so that it's not greater than MaxSpeed */   Real fBaseAngularWheelSpeed = Min<Real>(fHeadingLength, m_sWheelTurningParams.MaxSpeed);   /* Turning state switching conditions */   if(Abs(cHeadingAngle) <= m_sWheelTurningParams.NoTurnAngleThreshold) {      /* No Turn, heading angle very small */      m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::NO_TURN;   }   else if(Abs(cHeadingAngle) > m_sWheelTurningParams.HardTurnOnAngleThreshold) {      /* Hard Turn, heading angle very large */      m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::HARD_TURN;   }   else if(m_sWheelTurningParams.TurningMechanism == SWheelTurningParams::NO_TURN &&           Abs(cHeadingAngle) > m_sWheelTurningParams.SoftTurnOnAngleThreshold) {      /* Soft Turn, heading angle in between the two cases */      m_sWheelTurningParams.TurningMechanism = SWheelTurningParams::SOFT_TURN;   }   /* Wheel speeds based on current turning state */   Real fSpeed1, fSpeed2;   switch(m_sWheelTurningParams.TurningMechanism) {      case SWheelTurningParams::NO_TURN: {         /* Just go straight */         fSpeed1 = fBaseAngularWheelSpeed;         fSpeed2 = fBaseAngularWheelSpeed;         break;      }      case SWheelTurningParams::SOFT_TURN: {         /* Both wheels go straight, but one is faster than the other */         Real fSpeedFactor = (m_sWheelTurningParams.HardTurnOnAngleThreshold - Abs(cHeadingAngle)) / m_sWheelTurningParams.HardTurnOnAngleThreshold;         fSpeed1 = fBaseAngularWheelSpeed - fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);         fSpeed2 = fBaseAngularWheelSpeed + fBaseAngularWheelSpeed * (1.0 - fSpeedFactor);         break;      }      case SWheelTurningParams::HARD_TURN: {         /* Opposite wheel speeds */         fSpeed1 = -m_sWheelTurningParams.MaxSpeed;         fSpeed2 =  m_sWheelTurningParams.MaxSpeed;         break;      }   }   /* Apply the calculated speeds to the appropriate wheels */   Real fLeftWheelSpeed, fRightWheelSpeed;   if(cHeadingAngle > CRadians::ZERO) {      /* Turn Left */      fLeftWheelSpeed  = fSpeed1;      fRightWheelSpeed = fSpeed2;   }   else {      /* Turn Right */      fLeftWheelSpeed  = fSpeed2;      fRightWheelSpeed = fSpeed1;   }   /* Finally, set the wheel speeds */   m_pcWheels->SetLinearVelocity(fLeftWheelSpeed, fRightWheelSpeed);}

void Text::SetEffectStrokeThickness(int thickness){    strokeThickness_ = Abs(thickness);}

 bool CCylinder::Intersects(Real& f_t_on_ray,                            const CRay3& c_ray) {    /*     * This algorithm was adapted from     * http://www.realtimerendering.com/resources/GraphicsGems/gemsiv/ray_cyl.c     */    /* Vector from cylinder base to ray start */    CVector3 cCylBase2RayStart(c_ray.GetStart());    cCylBase2RayStart -= m_cBasePos;    /* Ray direction and length */    CVector3 cRayDir;    c_ray.GetDirection(cRayDir);    Real fRayLen = c_ray.GetLength();    /* Vector normal to cylinder axis and ray direction */    CVector3 cNormal(cRayDir);    cNormal.CrossProduct(m_cAxis);    Real fNormalLen = cNormal.Length();    /* Are cylinder axis and ray parallel? */    if(fNormalLen > 0) {       /* No, they aren't parallel */       /* Make normal have length 1 */       cNormal /= fNormalLen;       /* Calculate shortest distance between axis and ray        * by projecting cCylBase2RayStart onto cNormal */       Real fDist = Abs(cCylBase2RayStart.DotProduct(cNormal));       /* Is fDist smaller than the cylinder radius? */       if(fDist > m_fRadius) {          /* No, it's not, so there can't be any intersection */          return false;       }       /* If we get here, it's because the ray intersects the infinite cylinder */       /* Create a buffer for the 4 potential intersection points          (two on the sides, two on the bases) */       Real fPotentialT[4];       /* First, calculate the intersection points with the sides */       /* Calculate the midpoint between the two intersection points */       CVector3 cVec(cCylBase2RayStart);       cVec.CrossProduct(m_cAxis);       Real fMidPointDist = -cVec.DotProduct(cNormal) / fNormalLen;       /* Calculate the distance between the midpoint and the potential t's */       cVec = cNormal;       cVec.CrossProduct(m_cAxis);       cVec.Normalize();       Real fDeltaToMidPoint = Abs(Sqrt(Square(m_fRadius) - Square(fDist)) / cRayDir.DotProduct(cVec));       /* Calculate the potential t's on the infinite surface */       fPotentialT[0] = (fMidPointDist - fDeltaToMidPoint) / fRayLen;       fPotentialT[1] = (fMidPointDist + fDeltaToMidPoint) / fRayLen;       /* Make sure these t's correspond to points within the cylinder bases */       CVector3 cPoint;       c_ray.GetPoint(cPoint, fPotentialT[0]);       if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||          (cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {          fPotentialT[0] = -1;       }       c_ray.GetPoint(cPoint, fPotentialT[1]);       if((cPoint - m_cBasePos).DotProduct(m_cAxis) < 0 ||          (cPoint - (m_cBasePos + m_fHeight * m_cAxis)).DotProduct(m_cAxis) > 0) {          fPotentialT[1] = -1;       }       /* Check whether the ray is contained within the cylinder bases */       Real fDenominator = cRayDir.DotProduct(m_cAxis);       /* Is ray parallel to plane? */       if(Abs(fDenominator) > 1e-6) {          /* No, it's not parallel */          fDenominator *= fRayLen;          /* Bottom base */          fPotentialT[2] =             (m_cBasePos - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;          /* Top base */          fPotentialT[3] =             (m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;          /* Make sure these t's are within the cylinder surface */          c_ray.GetPoint(cPoint, fPotentialT[2]);          CVector3 cDiff = cPoint - m_cBasePos;          if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))             fPotentialT[2] = -1;          c_ray.GetPoint(cPoint, fPotentialT[3]);          cDiff = cPoint - m_cBasePos;          if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))             fPotentialT[3] = -1;       }       else {          /* Yes, it's parallel - discard the intersections */          fPotentialT[2] = -1.0;          fPotentialT[3] = -1.0;       }       /* Go through all the potential t's and get the best */       f_t_on_ray = 2.0;       for(UInt32 i = 0; i < 4; ++i) {          if(fPotentialT[i] > 0.0f) {             f_t_on_ray = Min(f_t_on_ray, fPotentialT[i]);          }       }       /* Return true only if the intersection point is within the ray limits */       return (f_t_on_ray < 1.0f);    }    else {       /* Yes, ray and axis are parallel */       /* Projection of cCylBase2RayStart onto the axis */       Real fProj = cCylBase2RayStart.DotProduct(m_cAxis);//.........这里部分代码省略.........

static Bool _DeformFn(BaseDocument *doc, BaseList2D *op, HairObject *hair, HairGuides *guides, Vector *padr, LONG cnt, LONG scnt){	LONG i,l;	BaseContainer *bc=op->GetDataInstance();	const SReal *pCombX=NULL,*pCombY=NULL,*pCombZ=NULL;	HairLibrary hlib;	RootObjectData rData;	hair->GetRootObject(NULL,NULL,&rData);	if (!rData.pObject) return TRUE;	Real strength=bc->GetReal(HAIR_DEFORMER_STRENGTH);	VertexMapTag *pVTag=(VertexMapTag*)bc->GetLink(HAIR_DEFORMER_COMB_X,doc,Tvertexmap);	if (pVTag && pVTag->GetObject()==rData.pObject) pCombX=pVTag->GetDataAddressR();	pVTag=(VertexMapTag*)bc->GetLink(HAIR_DEFORMER_COMB_Y,doc,Tvertexmap);	if (pVTag && pVTag->GetObject()==rData.pObject) pCombY=pVTag->GetDataAddressR();	pVTag=(VertexMapTag*)bc->GetLink(HAIR_DEFORMER_COMB_Z,doc,Tvertexmap);	if (pVTag && pVTag->GetObject()==rData.pObject) pCombZ=pVTag->GetDataAddressR();	if (!(pCombX || pCombY || pCombZ)) return TRUE;	const CPolygon *vadr=rData.pPolygon;	if (!padr || !vadr) return TRUE;	for (i=0;i<cnt;i++)	{		Vector comb,dn(DC);		HairRootData hroot=guides->GetRoot(i);		LONG p=hroot.m_ID;		Real s=hroot.m_S,t=hroot.m_T;		if (hroot.m_Type==HAIR_ROOT_TYPE_POLY)		{			if (pCombX) comb.x=hlib.MixST(s,t,pCombX[vadr[p].a],pCombX[vadr[p].b],pCombX[vadr[p].c],pCombX[vadr[p].d],vadr[p].c!=vadr[p].d)-0.5;			if (pCombY) comb.y=hlib.MixST(s,t,pCombY[vadr[p].a],pCombY[vadr[p].b],pCombY[vadr[p].c],pCombY[vadr[p].d],vadr[p].c!=vadr[p].d)-0.5;			if (pCombZ) comb.z=hlib.MixST(s,t,pCombZ[vadr[p].a],pCombZ[vadr[p].b],pCombZ[vadr[p].c],pCombZ[vadr[p].d],vadr[p].c!=vadr[p].d)-0.5;		}		else if (hroot.m_Type==HAIR_ROOT_TYPE_VERTEX)		{			if (pCombX) comb.x=pCombX[p];			if (pCombY) comb.y=pCombX[p];			if (pCombZ) comb.z=pCombX[p];		}		else			continue;		dn=!(padr[i*scnt+1]-padr[i*scnt]);		Real cs=Len(comb)*strength;		if (Abs(cs)<1e-5) continue;				comb=comb/cs;		dn=!Mix(dn,comb,cs);		Vector ax=comb%dn;		Real theta=dn*comb;		Matrix tm=RotAxisToMatrix(ax,theta);		for (l=1;l<scnt;l++)		{			padr[i*scnt+l]=((padr[i*scnt+l]-padr[i*scnt])^tm)+padr[i*scnt];		}	}	return TRUE;}

      const bool conjugate = ( shift0.imag() == -shift1.imag() );      if( !bothReal && !conjugate )          LogicError("Assumed shifts were either both real or conjugates");    )    if( n == 2 )    {        const Real& eta00 = H(0,0);        const Real& eta01 = H(0,1);        const Real& eta10 = H(1,0);        const Real& eta11 = H(1,1);        // It seems arbitrary whether the scale is computed relative        // to shift0 or shift1, but we follow LAPACK's convention.        // (While the choice is irrelevant for conjugate shifts, it is not for        //  real shifts)        const Real scale = OneAbs(eta00-shift1) + Abs(eta10);        if( scale == zero )        {            v[0] = v[1] = zero;        }        else        {            // Normalize the first column by the scale            Real eta10Scale = eta10 / scale;            v[0] = eta10Scale*eta01 +                   (eta00-shift0.real())*((eta00-shift1.real())/scale) -                   shift0.imag()*(shift1.imag()/scale);            v[1] = eta10Scale*(eta00+eta11-shift0.real()-shift1.real());        }    }    else

////#############################################################################//#############################################################################//UnitQuaternion&	UnitQuaternion::Subtract(		const UnitVector3D &end,		const UnitVector3D &start	){	Check_Pointer(this);	Check_Object(&start);	Check_Object(&end);	Vector3D		axis;	SinCosPair		delta;	delta.cosine = start*end;	//	//----------------------------------------------------------------------	// See if the vectors point in the same direction.  If so, return a null	// rotation	//----------------------------------------------------------------------	//	if (Close_Enough(delta.cosine, 1.0f))	{		x = 0.0f;		y = 0.0f;		z = 0.0f;		w = 1.0f;	}	//	//-------------------------------------------------------------------------	// See if the vectors directly oppose each other.  If so, pick the smallest	// axis coordinate and generate a vector along it.  Project this onto the	// base vector and subtract it out, leaving a perpendicular projection.	// Extend that out to unit length, then set the angle to PI	//-------------------------------------------------------------------------	//	else if (Close_Enough(delta.cosine, -1.0f))	{		//		//---------------------------		// Pick out the smallest axis		//---------------------------		//		int			smallest=0;		Scalar			value=2.0f;		for (int i=X_Axis; i<=Z_Axis; ++i)		{			if (Abs(start[i]) < value)			{				smallest = i;				value = Abs(start[i]);			}		}		//		//----------------------------------------		// Set up a vector along the selected axis		//----------------------------------------		//		axis.x = 0.0f;		axis.y = 0.0f;		axis.z = 0.0f;		axis[smallest] = 1.0f;		//		//-------------------------------------------------------------------		// If the value on that axis wasn't zero, subtract out the projection		//-------------------------------------------------------------------		//		if (!Small_Enough(value))		{			Vector3D t;			t.Multiply(start, start*axis);			axis.Subtract(axis, t);			axis.Normalize(axis);		}		//		//----------------------		// Convert to quaternion		//----------------------		//		x = axis.x;		y = axis.y;		z = axis.z;		w = 0.0f;	}	//	//--------------------------------------------------	// Otherwise, generate the cross product and unitize	//--------------------------------------------------//.........这里部分代码省略.........

void AABB::SetFrom(const OBB &obb){	vec halfSize = Abs(obb.axis[0]*obb.r[0]) + Abs(obb.axis[1]*obb.r[1]) + Abs(obb.axis[2]*obb.r[2]);	SetFromCenterAndSize(obb.pos, 2.f*halfSize);}

bool EqualRel(float a, float b, float maxRelError){	if (a == b) return true; // Handles the special case where a and b are both zero.	float relativeError = Abs((a-b)/Max(a, b));	return relativeError <= maxRelError;}

bool Cylinder::Intersects( const Segment& s, CollisionInfo* const pInfo /*= NULL*/ ) const{	Vector d = m_Point2 - m_Point1;		// Cylinder axis	Vector m = s.m_Point1 - m_Point1;	// Vector from cylinder base to segment base?	Vector n = s.m_Point2 - s.m_Point1;	// Segment vector	float md = m.Dot( d );	float nd = n.Dot( d );	float dd = d.Dot( d );	if( md < 0.0f && md + nd < 0.0f )	{		return false;	}	if( md > dd && md + nd > dd )	{		return false;	}	float nn = n.Dot( n );	float mn = m.Dot( n );	float a = dd * nn - nd * nd;	float k = m.Dot( m ) - m_Radius * m_Radius;	float c = dd * k - md * md;	float t = 0.0f;	if( Abs( a ) < SMALLER_EPSILON )	{		// Segment (n) runs parallel to cylinder axis (d)		if( c > 0.0f )		{			return false;		}		if( md < 0.0f )		{			t = -mn / nn;		}		else if( md > dd )		{			t = ( nd - mn ) / nn;		}		else		{			// TODO: This seems to be problematic (or getting here is indicative of an earlier problem)			WARNDESC( "THAT CYLINDER COLLISION BUG" );			t = 0.0f;		}		if( pInfo )		{			Vector Intersection = s.m_Point1 + t * n;			Vector PointOnLine = Line( m_Point1, m_Point2 - m_Point1 ).NearestPointTo( pInfo->m_Intersection );			Vector Normal = ( Intersection - PointOnLine ).GetNormalized();			pInfo->m_Collision = true;			pInfo->m_Intersection = Intersection;			pInfo->m_HitT = t;			pInfo->m_Plane = Plane( Normal, PointOnLine );		}		return true;	}	float b = dd * mn - nd * md;	float Discr = b * b - a * c;	if( Discr < 0.0f )	{		return false;	}	t = ( -b - SqRt( Discr ) ) / a;	if( t < 0.0f || t > 1.0f )	{		return false;	}	// Test endcaps--if we collide with them, count it as not colliding with cylinder	if( md + t * nd < 0.0f )	{		return false;		//// Segment outside cylinder on first side		//if( nd <= 0.0f )		//{		//	// Segment pointing away from endcap		//	return false;		//}		//float t2 = -md / nd;		//if( k + t2 * ( 2.0f * mn + t2 * nn ) > 0.0f )		//{		//	return false;		//}	}	else if( md + t * nd > dd )	{		return false;		//// Segment outside cylinder on second side//.........这里部分代码省略.........

static floatcalc_rank_and(float *w, tsvector * t, QUERYTYPE * q){	uint16	  **pos;	int			i,				k,				l,				p;	WordEntry  *entry;	WordEntryPos *post,			   *ct;	int4		dimt,				lenct,				dist;	float		res = -1.0;	ITEM	  **item;	int			size = q->size;	item = SortAndUniqItems(GETOPERAND(q), GETQUERY(q), &size);	if (size < 2)	{		pfree(item);		return calc_rank_or(w, t, q);	}	pos = (uint16 **) palloc(sizeof(uint16 *) * q->size);	memset(pos, 0, sizeof(uint16 *) * q->size);	*(uint16 *) POSNULL = lengthof(POSNULL) - 1;	WEP_SETPOS(POSNULL[1], MAXENTRYPOS - 1);	for (i = 0; i < size; i++)	{		entry = find_wordentry(t, q, item[i]);		if (!entry)			continue;		if (entry->haspos)			pos[i] = (uint16 *) _POSDATAPTR(t, entry);		else			pos[i] = (uint16 *) POSNULL;		dimt = *(uint16 *) (pos[i]);		post = (WordEntryPos *) (pos[i] + 1);		for (k = 0; k < i; k++)		{			if (!pos[k])				continue;			lenct = *(uint16 *) (pos[k]);			ct = (WordEntryPos *) (pos[k] + 1);			for (l = 0; l < dimt; l++)			{				for (p = 0; p < lenct; p++)				{					dist = Abs((int) WEP_GETPOS(post[l]) - (int) WEP_GETPOS(ct[p]));					if (dist || (dist == 0 && (pos[i] == (uint16 *) POSNULL || pos[k] == (uint16 *) POSNULL)))					{						float		curw;						if (!dist)							dist = MAXENTRYPOS;						curw = sqrt(wpos(post[l]) * wpos(ct[p]) * word_distance(dist));						res = (res < 0) ? curw : 1.0 - (1.0 - res) * (1.0 - curw);					}				}			}		}	}	pfree(pos);	pfree(item);	return res;}

/** The following code is from Christer Ericson's book Real-Time Collision Detection, pp. 101-106.    http://realtimecollisiondetection.net/ */bool OBB::Intersects(const OBB &b, float epsilon) const{    assume(pos.IsFinite());    assume(b.pos.IsFinite());    assume(float3::AreOrthonormal(axis[0], axis[1], axis[2]));    assume(float3::AreOrthonormal(b.axis[0], b.axis[1], b.axis[2]));    // Generate a rotation matrix that transforms from world space to this OBB's coordinate space.    float3x3 R;    for(int i = 0; i < 3; ++i)        for(int j = 0; j < 3; ++j)            R[i][j] = Dot(axis[i], b.axis[j]);    float3 t = b.pos - pos;    // Express the translation vector in a's coordinate frame.    t = float3(Dot(t, axis[0]), Dot(t, axis[1]), Dot(t, axis[2]));    float3x3 AbsR;    for(int i = 0; i < 3; ++i)        for(int j = 0; j < 3; ++j)            AbsR[i][j] = Abs(R[i][j]) + epsilon;    // Test the three major axes of this OBB.    for(int i = 0; i < 3; ++i)    {        float ra = r[i];        float rb = DOT3(b.r, AbsR[i]);        if (Abs(t[i]) > ra + rb)             return false;    }    // Test the three major axes of the OBB b.    for(int i = 0; i < 3; ++i)    {        float ra = r[0] * AbsR[0][i] + r[1] * AbsR[1][i] + r[2] * AbsR[2][i];        float rb = b.r[i];        if (Abs(t.x + R[0][i] + t.y * R[1][i] + t.z * R[2][i]) > ra + rb)            return false;    }    // Test the 9 different cross-axes.    // A.x <cross> B.x    float ra = r.y * AbsR[2][0] + r.z * AbsR[1][0];    float rb = b.r.y * AbsR[0][2] + b.r.z * AbsR[0][1];    if (Abs(t.z * R[1][0] - t.y * R[2][0]) > ra + rb)        return false;    // A.x < cross> B.y    ra = r.y * AbsR[2][1] + r.z * AbsR[1][1];    rb = b.r.x * AbsR[0][2] + b.r.z * AbsR[0][0];    if (Abs(t.z * R[1][1] - t.y * R[2][1]) > ra + rb)        return false;    // A.x <cross> B.z    ra = r.y * AbsR[2][2] + r.z * AbsR[1][2];    rb = b.r.x * AbsR[0][1] + b.r.y * AbsR[0][0];    if (Abs(t.z * R[1][22] - t.y * R[2][2]) > ra + rb)        return false;    // A.y <cross> B.x    ra = r.x * AbsR[2][0] + r.z * AbsR[0][0];    rb = b.r.y * AbsR[1][2] + b.r.z * AbsR[1][1];    if (Abs(t.x * R[2][0] - t.z * R[0][0]) > ra + rb)        return false;    // A.y <cross> B.y    ra = r.x * AbsR[2][1] + r.z * AbsR[0][1];    rb = b.r.x * AbsR[1][2] + b.r.z * AbsR[1][0];    if (Abs(t.x * R[2][1] - t.z * R[0][1]) > ra + rb)        return false;    // A.y <cross> B.z    ra = r.x * AbsR[2][2] + r.z * AbsR[0][2];    rb = b.r.x * AbsR[1][1] + b.r.y * AbsR[1][0];    if (Abs(t.x * R[2][2] - t.z * R[0][2]) > ra + rb)        return false;    // A.z <cross> B.x    ra = r.x * AbsR[1][0] + r.y * AbsR[0][0];    rb = b.r.y * AbsR[2][2] + b.r.z * AbsR[2][1];    if (Abs(t.y * R[0][0] - t.x * R[1][0]) > ra + rb)        return false;    // A.z <cross> B.y    ra = r.x * AbsR[1][1] + r.y * AbsR[0][1];    rb = b.r.x * AbsR[2][2] + b.r.z * AbsR[2][0];    if (Abs(t.y * R[0][1] - t.x * R[1][1]) > ra + rb)        return false;    // A.z <cross> B.z    ra = r.x * AbsR[1][2] + r.y * AbsR[0][2];    rb = b.r.x * AbsR[2][1] + b.r.y * AbsR[2][0];    if (Abs(t.y * R[0][2] - t.x * R[1][2]) > ra + rb)        return false;    // No separating axis exists, so the two OBB don't intersect.    return true;//.........这里部分代码省略.........

void CheckSticksHaveChanged(void){	#ifndef TESTING	static uint32 Now;	static boolean Change;	static uint8 c;	if ( F.FailsafesEnabled )	{		Now = mSClock();		if ( F.ReturnHome || F.Navigate  )		{			Change = true;			mS[RxFailsafeTimeout] = Now + RC_NO_CHANGE_TIMEOUT_MS;						F.ForceFailsafe = false;		}		else		{			if ( Now > mS[StickChangeUpdate] )			{				mS[StickChangeUpdate] = Now + 500;						Change = false;				for ( c = ThrottleC; c <= (uint8)RTHRC; c++ )				{					Change |= Abs( RC[c] - RCp[c]) > RC_STICK_MOVEMENT;					RCp[c] = RC[c];				}			}					if ( Change )			{				mS[RxFailsafeTimeout] = Now + RC_NO_CHANGE_TIMEOUT_MS;				mS[NavStateTimeout] = Now;				F.ForceFailsafe = false;					if ( FailState == MonitoringRx )				{					if ( F.LostModel )					{						Beeper_OFF;						F.LostModel = false;						DescentComp = 1;					}				}			}			else				if ( Now > mS[RxFailsafeTimeout] )				{					if ( !F.ForceFailsafe && ( State == InFlight ) )					{						//Stats[RCFailsafesS]++;						mS[NavStateTimeout] = Now + NAV_RTH_LAND_TIMEOUT_MS;						mS[DescentUpdate]  = Now + ALT_DESCENT_UPDATE_MS;						DescentComp = 1; // for no Baro case						F.ForceFailsafe = true;					}				}		}	}	else		F.ForceFailsafe = false;	#else	F.ForceFailsafe = false;	#endif // ENABLE_STICK_CHANGE_FAILSAFE} // CheckSticksHaveChanged

void LUMod( Matrix<F>& A,        Permutation& P,   const Matrix<F>& u,  const Matrix<F>& v,  bool conjugate,  Base<F> tau ){    DEBUG_CSE    typedef Base<F> Real;    const Int m = A.Height();    const Int n = A.Width();    const Int minDim = Min(m,n);    if( minDim != m )        LogicError("It is assumed that height(A) <= width(A)");    if( u.Height() != m || u.Width() != 1 )        LogicError("u is expected to be a conforming column vector");    if( v.Height() != n || v.Width() != 1 )        LogicError("v is expected to be a conforming column vector");    // w := inv(L) P u    auto w( u );    P.PermuteRows( w );    Trsv( LOWER, NORMAL, UNIT, A, w );    // Maintain an external vector for the temporary subdiagonal of U    Matrix<F> uSub;    Zeros( uSub, minDim-1, 1 );    // Reduce w to a multiple of e0    for( Int i=minDim-2; i>=0; --i )    {        // Decide if we should pivot the i'th and i+1'th rows of w        const F lambdaSub = A(i+1,i);        const F ups_ii = A(i,i);         const F omega_i = w(i);        const F omega_ip1 = w(i+1);        const Real rightTerm = Abs(lambdaSub*omega_i+omega_ip1);        const bool pivot = ( Abs(omega_i) < tau*rightTerm );        const Range<Int> indi( i, i+1 ),                         indip1( i+1, i+2 ),                         indB( i+2, m ),                         indR( i+1, n );        auto lBi   = A( indB,   indi   );        auto lBip1 = A( indB,   indip1 );        auto uiR   = A( indi,   indR   );        auto uip1R = A( indip1, indR   );        if( pivot )        {            // P := P_i P            P.Swap( i, i+1 );            // Simultaneously perform             //   U := P_i U and            //   L := P_i L P_i^T            //            // Then update            //     L := L T_{i,L}^{-1},            //     U := T_{i,L} U,             //     w := T_{i,L} P_i w,            // where T_{i,L} is the Gauss transform which zeros (P_i w)_{i+1}.            //             // More succinctly,            //     gamma    := w(i) / w(i+1),            //     w(i)     := w(i+1),             //     w(i+1)   := 0,            //     L(:,i)   += gamma L(:,i+1),            //     U(i+1,:) -= gamma U(i,:).            const F gamma = omega_i / omega_ip1;            const F lambda_ii = F(1) + gamma*lambdaSub;            A(i,  i) = gamma;            A(i+1,i) = 0;            auto lBiCopy = lBi;            Swap( NORMAL, lBi, lBip1 );            Axpy( gamma, lBiCopy, lBi );            auto uip1RCopy = uip1R;            RowSwap( A, i, i+1 );            Axpy( -gamma, uip1RCopy, uip1R );            // Force L back to *unit* lower-triangular form via the transform            //     L := L T_{i,U}^{-1} D^{-1},             // where D is diagonal and responsible for forcing L(i,i) and             // L(i+1,i+1) back to 1. The effect on L is:            //     eta       := L(i,i+1)/L(i,i),            //     L(:,i+1)  -= eta L(:,i),            //     delta_i   := L(i,i),            //     delta_ip1 := L(i+1,i+1),            //     L(:,i)   /= delta_i,            //     L(:,i+1) /= delta_ip1,            // while the effect on U is            //     U(i,:)   += eta U(i+1,:)            //     U(i,:)   *= delta_i,            //     U(i+1,:) *= delta_{i+1},            // and the effect on w is            //     w(i) *= delta_i.//.........这里部分代码省略.........

inline typename Base<F>::typeHermitianFrobeniusNorm( UpperOrLower uplo, const DistMatrix<F>& A ){#ifndef RELEASE    PushCallStack("internal::HermitianFrobeniusNorm");#endif    typedef typename Base<F>::type R;    if( A.Height() != A.Width() )        throw std::logic_error("Hermitian matrices must be square.");    const int r = A.Grid().Height();    const int c = A.Grid().Width();    const int colShift = A.ColShift();    const int rowShift = A.RowShift();    R localScale = 0;    R localScaledSquare = 1;    const int localWidth = A.LocalWidth();    if( uplo == UPPER )    {        for( int jLocal=0; jLocal<localWidth; ++jLocal )        {            int j = rowShift + jLocal*c;            int numUpperRows = LocalLength(j+1,colShift,r);            for( int iLocal=0; iLocal<numUpperRows; ++iLocal )            {                int i = colShift + iLocal*r;                const R alphaAbs = Abs(A.GetLocal(iLocal,jLocal));                if( alphaAbs != 0 )                {                    if( alphaAbs <= localScale )                    {                        const R relScale = alphaAbs/localScale;                        if( i != j )                            localScaledSquare += 2*relScale*relScale;                        else                            localScaledSquare += relScale*relScale;                    }                    else                    {                        const R relScale = localScale/alphaAbs;                        if( i != j )                            localScaledSquare =                                 localScaledSquare*relScale*relScale + 2;                        else                            localScaledSquare =                                 localScaledSquare*relScale*relScale + 1;                        localScale = alphaAbs;                    }                }            }        }    }    else    {        for( int jLocal=0; jLocal<localWidth; ++jLocal )        {            int j = rowShift + jLocal*c;            int numStrictlyUpperRows = LocalLength(j,colShift,r);            for( int iLocal=numStrictlyUpperRows;                  iLocal<A.LocalHeight(); ++iLocal )            {                int i = colShift + iLocal*r;                const R alphaAbs = Abs(A.GetLocal(iLocal,jLocal));                if( alphaAbs != 0 )                {                    if( alphaAbs <= localScale )                    {                        const R relScale = alphaAbs/localScale;                        if( i != j )                            localScaledSquare += 2*relScale*relScale;                        else                            localScaledSquare += relScale*relScale;                    }                    else                    {                        const R relScale = localScale/alphaAbs;                        if( i != j )                            localScaledSquare =                                 localScaledSquare*relScale*relScale + 2;                        else                            localScaledSquare =                                localScaledSquare*relScale*relScale + 1;                         localScale = alphaAbs;                    }                }            }        }    }    // Find the maximum relative scale    R scale;    mpi::AllReduce( &localScale, &scale, 1, mpi::MAX, A.Grid().VCComm() );    R norm = 0;    if( scale != 0 )    {        // Equilibrate our local scaled sum to the maximum scale//.........这里部分代码省略.........

static TDStrResult GetFinalDocumentImportances(    const TVector<TVector<double>>& rawImportances,    EDocumentStrengthType docImpMethod,    int topSize,    EImportanceValuesSign importanceValuesSign) {    const ui32 trainDocCount = rawImportances.size();    Y_ASSERT(rawImportances.size() != 0);    const ui32 testDocCount = rawImportances[0].size();    TVector<TVector<double>> preprocessedImportances;    if (docImpMethod == EDocumentStrengthType::Average) {        preprocessedImportances = TVector<TVector<double>>(1, TVector<double>(trainDocCount));        for (ui32 trainDocId = 0; trainDocId < trainDocCount; ++trainDocId) {            for (ui32 testDocId = 0; testDocId < testDocCount; ++testDocId) {                preprocessedImportances[0][trainDocId] += rawImportances[trainDocId][testDocId];            }        }        for (ui32 trainDocId = 0; trainDocId < trainDocCount; ++trainDocId) {            preprocessedImportances[0][trainDocId] /= testDocCount;        }    } else {        Y_ASSERT(docImpMethod == EDocumentStrengthType::PerObject || docImpMethod == EDocumentStrengthType::Raw);        preprocessedImportances = TVector<TVector<double>>(testDocCount, TVector<double>(trainDocCount));        for (ui32 trainDocId = 0; trainDocId < trainDocCount; ++trainDocId) {            for (ui32 testDocId = 0; testDocId < testDocCount; ++testDocId) {                preprocessedImportances[testDocId][trainDocId] = rawImportances[trainDocId][testDocId];            }        }    }    TDStrResult result(preprocessedImportances.size());    for (ui32 testDocId = 0; testDocId < preprocessedImportances.size(); ++testDocId) {        TVector<double>& preprocessedImportancesRef = preprocessedImportances[testDocId];        const ui32 docCount = preprocessedImportancesRef.size();        TVector<ui32> indices(docCount);        std::iota(indices.begin(), indices.end(), 0);        if (docImpMethod != EDocumentStrengthType::Raw) {            Sort(indices.begin(), indices.end(), [&](ui32 first, ui32 second) {                return Abs(preprocessedImportancesRef[first]) > Abs(preprocessedImportancesRef[second]);            });        }        std::function<bool(double)> predicate;        if (importanceValuesSign == EImportanceValuesSign::Positive) {            predicate = [](double v){return v > 0;};        } else if (importanceValuesSign == EImportanceValuesSign::Negative) {            predicate = [](double v){return v < 0;};        } else {            Y_ASSERT(importanceValuesSign == EImportanceValuesSign::All);            predicate = [](double){return true;};        }        int currentSize = 0;        for (ui32 i = 0; i < docCount; ++i) {            if (currentSize == topSize) {                break;            }            if (predicate(preprocessedImportancesRef[indices[i]])) {                result.Scores[testDocId].push_back(preprocessedImportancesRef[indices[i]]);                result.Indices[testDocId].push_back(indices[i]);            }            ++currentSize;        }    }    return result;}

inline typename Base<F>::type HermitianFrobeniusNorm( UpperOrLower uplo, const Matrix<F>& A ){#ifndef RELEASE    PushCallStack("internal::HermitianFrobeniusNorm");#endif    typedef typename Base<F>::type R;    if( A.Height() != A.Width() )        throw std::logic_error("Hermitian matrices must be square.");    R scale = 0;    R scaledSquare = 1;    const int height = A.Height();    const int width = A.Width();    if( uplo == UPPER )    {        for( int j=0; j<width; ++j )        {            for( int i=0; i<j; ++i )            {                const R alphaAbs = Abs(A.Get(i,j));                if( alphaAbs != 0 )                {                    if( alphaAbs <= scale )                    {                        const R relScale = alphaAbs/scale;                        scaledSquare += 2*relScale*relScale;                    }                    else                    {                        const R relScale = scale/alphaAbs;                        scaledSquare = scaledSquare*relScale*relScale + 2;                        scale = alphaAbs;                    }                }            }            const R alphaAbs = Abs(A.Get(j,j));            if( alphaAbs != 0 )            {                if( alphaAbs <= scale )                {                    const R relScale = alphaAbs/scale;                    scaledSquare += relScale*relScale;                }                else                {                    const R relScale = scale/alphaAbs;                    scaledSquare = scaledSquare*relScale*relScale + 1;                    scale = alphaAbs;                }            }        }    }    else    {        for( int j=0; j<width; ++j )        {            for( int i=j+1; i<height; ++i )            {                const R alphaAbs = Abs(A.Get(i,j));                if( alphaAbs != 0 )                {                    if( alphaAbs <= scale )                    {                        const R relScale = alphaAbs/scale;                        scaledSquare += 2*relScale*relScale;                    }                    else                    {                        const R relScale = scale/alphaAbs;                        scaledSquare = scaledSquare*relScale*relScale + 2;                        scale = alphaAbs;                    }                }            }            const R alphaAbs = Abs(A.Get(j,j));            if( alphaAbs != 0 )            {                if( alphaAbs <= scale )                {                       const R relScale = alphaAbs/scale;                    scaledSquare += relScale*relScale;                }                   else                {                    const R relScale = scale/alphaAbs;                    scaledSquare = scaledSquare*relScale*relScale + 1;                    scale = alphaAbs;                }            }        }    }    const R norm = scale*Sqrt(scaledSquare);#ifndef RELEASE    PopCallStack();#endif    return norm;}

/** For reference, see http://realtimecollisiondetection.net/blog/?p=20 . */Sphere Sphere::OptimalEnclosingSphere(const vec &a, const vec &b, const vec &c){	Sphere sphere;	vec ab = b-a;	vec ac = c-a;	float s, t;	bool areCollinear = ab.Cross(ac).LengthSq() < 1e-4f; // Manually test that we don't try to fit sphere to three collinear points.	bool success = !areCollinear && FitSphereThroughPoints(ab, ac, s, t);	if (!success || Abs(s) > 10000.f || Abs(t) > 10000.f) // If s and t are very far from the triangle, do a manual box fitting for numerical stability.	{		vec minPt = Min(a, b, c);		vec maxPt = Max(a, b, c);		sphere.pos = (minPt + maxPt) * 0.5f;		sphere.r = sphere.pos.Distance(minPt);	}	else if (s < 0.f)	{		sphere.pos = (a + c) * 0.5f;		sphere.r = a.Distance(c) * 0.5f;		sphere.r = Max(sphere.r, b.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.	}	else if (t < 0.f)	{		sphere.pos = (a + b) * 0.5f;		sphere.r = a.Distance(b) * 0.5f;		sphere.r = Max(sphere.r, c.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.	}	else if (s+t > 1.f)	{		sphere.pos = (b + c) * 0.5f;		sphere.r = b.Distance(c) * 0.5f;		sphere.r = Max(sphere.r, a.Distance(sphere.pos)); // For numerical stability, expand the radius of the sphere so it certainly contains the third point.	}	else	{		const vec center = s*ab + t*ac;		sphere.pos = a + center;		// Mathematically, the following would be correct, but it suffers from floating point inaccuracies,		// since it only tests distance against one point.		//sphere.r = center.Length();		// For robustness, take the radius to be the distance to the farthest point (though the distance are all		// equal).		sphere.r = Sqrt(Max(sphere.pos.DistanceSq(a), sphere.pos.DistanceSq(b), sphere.pos.DistanceSq(c)));	}	// Allow floating point inconsistency and expand the radius by a small epsilon so that the containment tests	// really contain the points (note that the points must be sufficiently near enough to the origin)	sphere.r += 2.f * sEpsilon; // We test against one epsilon, so expand by two epsilons.#ifdef MATH_ASSERT_CORRECTNESS	if (!sphere.Contains(a, sEpsilon) || !sphere.Contains(b, sEpsilon) || !sphere.Contains(c, sEpsilon))	{		LOGE("Pos: %s, r: %f", sphere.pos.ToString().c_str(), sphere.r);		LOGE("A: %s, dist: %f", a.ToString().c_str(), a.Distance(sphere.pos));		LOGE("B: %s, dist: %f", b.ToString().c_str(), b.Distance(sphere.pos));		LOGE("C: %s, dist: %f", c.ToString().c_str(), c.Distance(sphere.pos));		mathassert(false);	}#endif	return sphere;}

   This file is part of Elemental and is under the BSD 2-Clause License,    which can be found in the LICENSE file in the root directory, or at    http://opensource.org/licenses/BSD-2-Clause*/#include "El.hpp"namespace El {template<typename F> void Fiedler( Matrix<F>& A, const vector<F>& c ){    DEBUG_ONLY(CSE cse("Fiedler"))    const Int n = c.size();    A.Resize( n, n );    auto fiedlerFill = [&]( Int i, Int j ) { return Abs(c[i]-c[j]); };    IndexDependentFill( A, function<F(Int,Int)>(fiedlerFill) );}template<typename F>void Fiedler( AbstractDistMatrix<F>& A, const vector<F>& c ){    DEBUG_ONLY(CSE cse("Fiedler"))    const Int n = c.size();    A.Resize( n, n );    auto fiedlerFill = [&]( Int i, Int j ) { return Abs(c[i]-c[j]); };    IndexDependentFill( A, function<F(Int,Int)>(fiedlerFill) );}template<typename F>void Fiedler( AbstractBlockDistMatrix<F>& A, const vector<F>& c )

void FoxLi( ElementalMatrix<Complex<Real>>& APre, Int n, Real omega ){    DEBUG_CSE    typedef Complex<Real> C;    const Real pi = 4*Atan( Real(1) );    const C phi = Sqrt( C(0,omega/pi) );     DistMatrixWriteProxy<C,C,MC,MR> AProx( APre );    auto& A = AProx.Get();        // Compute Gauss quadrature points and weights    const Grid& g = A.Grid();    DistMatrix<Real,VR,STAR> d(g), e(g);     Zeros( d, n, 1 );    e.Resize( n-1, 1 );    auto& eLoc = e.Matrix();    for( Int iLoc=0; iLoc<e.LocalHeight(); ++iLoc )    {        const Int i = e.GlobalRow(iLoc);        const Real betaInv = 2*Sqrt(1-Pow(i+Real(1),-2)/4);        eLoc(iLoc) = 1/betaInv;    }    DistMatrix<Real,VR,STAR> x(g);    DistMatrix<Real,STAR,VR> Z(g);    HermitianTridiagEig( d, e, x, Z, UNSORTED );    auto z = Z( IR(0), ALL );    DistMatrix<Real,STAR,VR> sqrtWeights( z );    auto& sqrtWeightsLoc = sqrtWeights.Matrix();    for( Int jLoc=0; jLoc<sqrtWeights.LocalWidth(); ++jLoc )        sqrtWeightsLoc(0,jLoc) = Sqrt(Real(2))*Abs(sqrtWeightsLoc(0,jLoc));    herm_eig::Sort( x, sqrtWeights, ASCENDING );    // Form the integral operator    A.Resize( n, n );    DistMatrix<Real,MC,STAR> x_MC( A.Grid() );    DistMatrix<Real,MR,STAR> x_MR( A.Grid() );    x_MC.AlignWith( A );     x_MR.AlignWith( A );    x_MC = x;    x_MR = x;    auto& ALoc = A.Matrix();    auto& x_MCLoc = x_MC.Matrix();    auto& x_MRLoc = x_MR.Matrix();    for( Int jLoc=0; jLoc<A.LocalWidth(); ++jLoc )    {        for( Int iLoc=0; iLoc<A.LocalHeight(); ++iLoc )        {            const Real diff = x_MCLoc(iLoc)-x_MRLoc(jLoc);            const Real theta = -omega*Pow(diff,2);            const Real realPart = Cos(theta);            const Real imagPart = Sin(theta);            ALoc(iLoc,jLoc) = phi*C(realPart,imagPart);        }    }    // Apply the weighting    DistMatrix<Real,VR,STAR> sqrtWeightsTrans(g);    Transpose( sqrtWeights, sqrtWeightsTrans );    DiagonalScale( LEFT, NORMAL, sqrtWeightsTrans, A );    DiagonalScale( RIGHT, NORMAL, sqrtWeightsTrans, A );}

 static bool Compare ( const IndexValuePair<R>& a, const IndexValuePair<R>& b ) { return Abs(a.value) < Abs(b.value); }

TWxTestResult WxTest(const TVector<double>& baseline,                     const TVector<double>& test) {    TVector<double> diffs;    for (ui32 i = 0; i < baseline.size(); i++) {        const double i1 = baseline[i];        const double i2 = test[i];        const double diff = i1 - i2;        if (diff != 0) {            diffs.push_back(diff);        }    }    if (diffs.size() < 2) {        TWxTestResult result;        result.PValue = 0.5;        result.WMinus = result.WPlus = 0;        return result;    }    Sort(diffs.begin(), diffs.end(), [&](double x, double y) {        return Abs(x) < Abs(y);    });    double w_plus = 0;    double w_minus = 0;    double n = diffs.size();    for (int i = 0; i < n; ++i) {        double sum = 0;        double weight = 0;        int j = i;        double signPlus = 0;        double signMinus = 0;        for (j = i; j < n && diffs[j] == diffs[i]; ++j) {            sum += (j + 1);            ++weight;            signPlus += diffs[i] >= 0;            signMinus += diffs[i] < 0;        }        const double meanRank = sum / weight;        w_plus += signPlus * meanRank;        w_minus += signMinus * meanRank;        i = j - 1;    }    TWxTestResult result;    result.WPlus = w_plus;    result.WMinus = w_minus;    const double w = result.WPlus - result.WMinus;    if (n > 16) {        double z = w / sqrt(n * (n + 1) * (2 * n + 1) * 1.0 / 6);        result.PValue = 2 * (1.0 - NormalCDF(Abs(z)));    } else {        result.PValue = 2 * CalcLevelOfSignificanceWXMPSR(Abs(w), (int) n);    }    result.PValue = 1.0 - result.PValue;    return result;}

//.........这里部分代码省略.........	left = v->spl_left;	v->spl_nleft = 0;	right = v->spl_right;	v->spl_nright = 0;	if (seed_1 == 0 || seed_2 == 0)	{		seed_1 = 1;		seed_2 = 2;	}	datum_alpha = GETENTRY(entryvec, seed_1);	datum_l = copy_intArrayType(datum_alpha);	rt__int_size(datum_l, &size_l);	datum_beta = GETENTRY(entryvec, seed_2);	datum_r = copy_intArrayType(datum_beta);	rt__int_size(datum_r, &size_r);	maxoff = OffsetNumberNext(maxoff);	/*	 * sort entries	 */	costvector = (SPLITCOST *) palloc(sizeof(SPLITCOST) * maxoff);	for (i = FirstOffsetNumber; i <= maxoff; i = OffsetNumberNext(i))	{		costvector[i - 1].pos = i;		datum_alpha = GETENTRY(entryvec, i);		union_d = inner_int_union(datum_l, datum_alpha);		rt__int_size(union_d, &size_alpha);		pfree(union_d);		union_d = inner_int_union(datum_r, datum_alpha);		rt__int_size(union_d, &size_beta);		pfree(union_d);		costvector[i - 1].cost = Abs((size_alpha - size_l) - (size_beta - size_r));	}	qsort((void *) costvector, maxoff, sizeof(SPLITCOST), comparecost);	/*	 * Now split up the regions between the two seeds.  An important property	 * of this split algorithm is that the split vector v has the indices of	 * items to be split in order in its left and right vectors.  We exploit	 * this property by doing a merge in the code that actually splits the	 * page.	 *	 * For efficiency, we also place the new index tuple in this loop. This is	 * handled at the very end, when we have placed all the existing tuples	 * and i == maxoff + 1.	 */	for (j = 0; j < maxoff; j++)	{		i = costvector[j].pos;		/*		 * If we've already decided where to place this item, just put it on		 * the right list.  Otherwise, we need to figure out which page needs		 * the least enlargement in order to store the item.		 */		if (i == seed_1)		{			*left++ = i;			v->spl_nleft++;			continue;		}

inline BOOL EpsilonEq(const Type &a, const Type &b) { return Abs(a-b)<=BSP_EPSILON; };

Quaternion CreateDiagonalizer(Mat3Param matrix){  const unsigned cMaxSteps = 50;  const float cThetaLimit = 1.0e6f;  Quaternion quat(0.0f, 0.0f, 0.0f, 1.0f);  Matrix3 quatMatrix;  Matrix3 diagMatrix;  for(unsigned i = 0; i < cMaxSteps; ++i)  {    ToMatrix3(quat, &quatMatrix);    diagMatrix = Concat(Concat(quatMatrix, matrix), quatMatrix.Transposed());    //Elements not on the diagonal    Vector3 offDiag(diagMatrix(1, 2), diagMatrix(0, 2), diagMatrix(0, 1));    //Magnitude of the off-diagonal elements    Vector3 magDiag = Abs(offDiag);    //Index of the largest element     unsigned k = ((magDiag.x > magDiag.y) && (magDiag.x > magDiag.z)) ? 0 :             ((magDiag.y > magDiag.z) ? 1 : 2);    unsigned k1 = (k + 1) % 3;    unsigned k2 = (k + 2) % 3;    //Diagonal already    if(offDiag[k] == 0.0f)    {      break;    }    float theta = (diagMatrix(k2, k2) - diagMatrix(k1, k1)) /                  (2.0f * offDiag[k]);    float sign = Math::GetSign(theta);        //Make theta positive    theta *= sign;    //Large term in T    float thetaTerm = theta < 1e6f ? Math::Sqrt(Math::Sq(theta) + 1.0f)                                       : theta;    //Sign(T) / (|T| + sqrt(T^2 + 1))    float t = sign / (theta + thetaTerm);    //c = 1 / (t^2 + 1)      t = s / c    float c = 1.0f / Math::Sqrt(Math::Sq(t) + 1.0f);    //No room for improvement - reached machine precision.    if(c == 1.0f)    {      break;    }    //Jacobi rotation for this iteration    Quaternion jacobi(0.0f, 0.0f, 0.0f, 0.0f);    //Using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)    jacobi[k] = sign * Math::Sqrt((1.0f - c) / 2.0f);    //Since our quat-to-matrix convention was for v*M instead of M*v    jacobi.w = Math::Sqrt(1.0f - Math::Sq(jacobi[k]));    //Reached limits of floating point precision    if(jacobi.w == 1.0f)    {      break;    }    quat *= jacobi;    Normalize(&quat);  }  return quat;}

inline BOOL EpsilonNe(const Type &a, const Type &b) { return Abs(a-b)> BSP_EPSILON; };

//.........这里部分代码省略.........//				HandoverFlag[1]=1;//				HandoverFlag[2]=0;//				PoleFlag = 1;//				SwingFlag = 1;			}			if(Button9_Off)			{//				HandoverFlag[0]=0;//				HandoverFlag[1]=0;//				HandoverFlag[2]=0;//				PoleFlag = 0;//				SwingFlag = 0;			}						if(XY_Only)			{			//	Handle_Speed_X=0;				Handle_Speed_Rotation=0;			}						if(GROUND==RED_GROUND)			{				Handle_Speed_X = -Handle_Speed_X;				Handle_Speed_Y = -Handle_Speed_Y;			}						if(x_con_flag)			{				temp_point.x = GPS.position.x + Handle_Speed_X*cos(GPS.radian);				temp_point.y = GPS.position.y + Handle_Speed_X*sin(GPS.radian);								SetSpeed(Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);								EasyLine(temp_point,GPS.radian,Abs(Handle_Speed_X));			}			else 				SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);          //SetSpeed(Speed_X,Speed_Y,Speed_Rotation+Handle_Speed_Rotation);   //手柄前后出现问题，先无视之。。。						/*********************************上层动作控制***********************************/						//------------单键控制电机------------//			if(Button1_Up && Button7_Off && flag[1]==0)			{				flag[1]=1;			    Moto_Set_GSpeed(W_MOTOR1_OLD_ID, 4*Speed_M);			}			if(Button1_Down && Button7_Off && flag[1]==0)			{				flag[1]=1;			    Moto_Set_GSpeed(W_MOTOR1_OLD_ID, -4*Speed_M);			}			if(Button1_Off && (flag[1]==1))			{				flag[1]=0;				Moto_Stop(W_MOTOR1_OLD_ID);			}						if(Button2_Up && Button7_Off && flag[2]==0)			{				flag[2]=1;			    Moto_Set_GSpeed(W_MOTOR2_OLD_ID, -3*Speed_M);			}			if(Button2_Down && Button7_Off && flag[2]==0)			{

TBool	CT_ActiveRConsoleRead::KickStartL(const TDesC& aSection, const TInt aAsyncErrorIndex, RConsole& aConsole)/** * Kick Start the object and set up intials *	@param		aSection			The section in the ini containing data for the command * 	@param		aAsyncErrorIndex	Command index for async calls to return errors to *	@param		aConsole			The RConsole object */	{	TBuf<KMaxTestExecuteCommandLength>	tempStore;	iSection.Set(aSection);	iColourValueBlack	=KBlack;	iColourValueWhite	=KWhite;	iDataWrapperBase.GetUint8FromConfig(iSection, KFldColourBlack(), iColourValueBlack);	iDataWrapperBase.GetUint8FromConfig(iSection, KFldColourWhite(), iColourValueWhite);	iTimeOut=KDefaultTimeout;	iDataWrapperBase.GetIntFromConfig(iSection, KFldTimeout(), iTimeOut);	iErrorMargin=0;	iDataWrapperBase.GetIntFromConfig(iSection, KFldErrorMargin(), iErrorMargin);	iHasExitKeyCode=iDataWrapperBase.GetHexFromConfig(iSection, KFldExitKeyCode(), iExitKeyCode);	iHasExitRectangle=iDataWrapperBase.GetRectFromConfig(iSection, KFldExitRectangle(), iExitRectangle);	if ( iHasExitRectangle )		{		//	Draw rectangle		TInt	height =Abs(iExitRectangle.iBr.iY-iExitRectangle.iTl.iY);		TInt	width	=Abs(iExitRectangle.iBr.iX-iExitRectangle.iTl.iX);		CDrawUtils::DrawSquareUtility(iExitRectangle.iTl, height, width, iColourValueWhite);		}	iEvent.Reset();	TEventConfig	config;	TInt			eventIndex=0;	TBool			dataOk=ETrue;	TBool			moreData=ETrue;	while ( moreData )		{		tempStore.Format(KFldEventType, ++eventIndex);		moreData=iDataWrapperBase.GetEnumFromConfig(iSection, tempStore, iEnumRawEventTable, config.iEventType);		if ( moreData )			{			tempStore.Format(KFldEventOccurance, eventIndex);			TInt	eventOccurance=EEventOccuranceOnce;			iDataWrapperBase.GetEnumFromConfig(iSection, tempStore, iEnumEventOccuranceTable, eventOccurance);			config.iEventOccurance=(TEventOccurance)eventOccurance;						tempStore.Format(KFldDataVerify, eventIndex);			config.iDataVerify=EFalse;			iDataWrapperBase.GetBoolFromConfig(iSection, tempStore, config.iDataVerify);			tempStore.Format(KFldDataDraw, eventIndex);			config.iDataDraw=EFalse;			iDataWrapperBase.GetBoolFromConfig(iSection, tempStore, config.iDataDraw);			iEvent.AppendL(config);			dataOk=ETrue;			}		}	//	If -1 then we have an umlimited number of test(s) that completes with an exit event	//	which can be an exit key code or a pen event in the exit rectangle	iNumberOfTests=-1;	iDataWrapperBase.GetIntFromConfig(iSection, KFldTests(), iNumberOfTests);	if ( dataOk )		{		iTestIndex=0;		dataOk=KickNext(aAsyncErrorIndex, aConsole);		}	return dataOk;	}

/** Initializes the XOSC32K crystal failure detector, and starts it. * *  /param[in]  ok_callback    Callback function to run upon XOSC32K operational *  /param[in]  fail_callback  Callback function to run upon XOSC32K failure */static void init_xosc32k_fail_detector(		const tc_callback_t ok_callback,		const tc_callback_t fail_callback){	/* TC pairs share the same clock, ensure reference and crystal timers use	 * different clocks */	Assert(Abs(_tc_get_inst_index(CONF_TC_OSC32K) -			_tc_get_inst_index(CONF_TC_XOSC32K)) >= 2);	/* The crystal detection cycle count must be less than the reference cycle	 * count, so that the reference timer is periodically reset before expiry */	Assert(CRYSTAL_RESET_CYCLES < CRYSTAL_FAIL_CYCLES);	/* Must use different clock generators for the crystal and reference, must	 * not be CPU generator 0 */	Assert(GCLK_GENERATOR_XOSC32K != GCLK_GENERATOR_OSC32K);	Assert(GCLK_GENERATOR_XOSC32K != GCLK_GENERATOR_0);	Assert(GCLK_GENERATOR_OSC32K  != GCLK_GENERATOR_0);	/* Configure and enable the XOSC32K GCLK generator */	struct system_gclk_gen_config xosc32k_gen_conf;	system_gclk_gen_get_config_defaults(&xosc32k_gen_conf);	xosc32k_gen_conf.source_clock = SYSTEM_CLOCK_SOURCE_XOSC32K;	system_gclk_gen_set_config(GCLK_GENERATOR_XOSC32K, &xosc32k_gen_conf);	system_gclk_gen_enable(GCLK_GENERATOR_XOSC32K);	/* Configure and enable the reference clock GCLK generator */	struct system_gclk_gen_config ref_gen_conf;	system_gclk_gen_get_config_defaults(&ref_gen_conf);	ref_gen_conf.source_clock = SYSTEM_CLOCK_SOURCE_OSC32K;	system_gclk_gen_set_config(GCLK_GENERATOR_OSC32K, &ref_gen_conf);	system_gclk_gen_enable(GCLK_GENERATOR_OSC32K);	/* Set up crystal counter - when target count elapses, trigger event */	struct tc_config tc_xosc32k_conf;	tc_get_config_defaults(&tc_xosc32k_conf);	tc_xosc32k_conf.clock_source = GCLK_GENERATOR_XOSC32K;	tc_xosc32k_conf.counter_16_bit.compare_capture_channel[0] =			CRYSTAL_RESET_CYCLES;	tc_xosc32k_conf.wave_generation = TC_WAVE_GENERATION_MATCH_FREQ;	tc_init(&tc_xosc32k, CONF_TC_XOSC32K, &tc_xosc32k_conf);	/* Set up reference counter - when event received, restart */	struct tc_config tc_osc32k_conf;	tc_get_config_defaults(&tc_osc32k_conf);	tc_osc32k_conf.clock_source = GCLK_GENERATOR_OSC32K;	tc_osc32k_conf.counter_16_bit.compare_capture_channel[0] =			CRYSTAL_FAIL_CYCLES;	tc_osc32k_conf.wave_generation = TC_WAVE_GENERATION_MATCH_FREQ;	tc_init(&tc_osc32k, CONF_TC_OSC32K, &tc_osc32k_conf);	/* Configure event channel and link it to the xosc32k counter */	struct events_config config;	struct events_resource event;	events_get_config_defaults(&config);	config.edge_detect  = EVENTS_EDGE_DETECT_NONE;	config.generator    = CONF_EVENT_GENERATOR_ID;	config.path         = EVENTS_PATH_ASYNCHRONOUS;	events_allocate(&event, &config);	/* Configure event user and link it to the osc32k counter */	events_attach_user(&event, CONF_EVENT_USED_ID);	/* Enable event generation for crystal counter */	struct tc_events tc_xosc32k_events = { .generate_event_on_overflow = true };	tc_enable_events(&tc_xosc32k, &tc_xosc32k_events);	/* Enable event reception for reference counter */	struct tc_events tc_osc32k_events = { .on_event_perform_action = true };	tc_osc32k_events.event_action = TC_EVENT_ACTION_RETRIGGER;	tc_enable_events(&tc_osc32k, &tc_osc32k_events);	/* Enable overflow callback for the crystal counter - if crystal count	 * has been reached, crystal is operational */	tc_register_callback(&tc_xosc32k, ok_callback, TC_CALLBACK_CC_CHANNEL0);	tc_enable_callback(&tc_xosc32k, TC_CALLBACK_CC_CHANNEL0);	/* Enable compare callback for the reference counter - if reference count	 * has been reached, crystal has failed */	tc_register_callback(&tc_osc32k, fail_callback, TC_CALLBACK_CC_CHANNEL0);	tc_enable_callback(&tc_osc32k, TC_CALLBACK_CC_CHANNEL0);	/* Start both crystal and reference counters */	tc_enable(&tc_xosc32k);	tc_enable(&tc_osc32k);}/** Main application entry point. */int main(void){	system_init();	system_flash_set_waitstates(2);	init_osc32k();	init_xosc32k();//.........这里部分代码省略.........

int checkIrregFabCopy(const Box& a_domain){  int eekflag = 0;  int nvar = 1;  Interval interv(0,0);  int nghost= 0;  IntVect ivgh= IntVect::Zero;  Real tolerance = PolyGeom::getTolerance();  const EBIndexSpace* const ebisPtr = Chombo_EBIS::instance();  int numlevels = ebisPtr->numLevels();  Box levelDomain = a_domain;  for (int ilev = 0; ilev < numlevels-1; ilev++)    {      DisjointBoxLayout dblOne, dblTwo;      int maxsizeOne = 4;      int maxsizeTwo = 2;      makeLayout(dblOne, levelDomain, maxsizeOne);      makeLayout(dblTwo, levelDomain, maxsizeTwo);      EBISLayout ebislOne, ebislTwo;      makeEBISL(ebislOne, dblOne, levelDomain, nghost);      makeEBISL(ebislTwo, dblTwo, levelDomain, nghost);      LayoutData<IntVectSet> ldIVSOne(dblOne);      LayoutData<IntVectSet> ldIVSTwo(dblTwo);      for (DataIterator dit = dblOne.dataIterator(); dit.ok(); ++dit)        ldIVSOne[dit()] = IntVectSet(dblOne.get(dit()));      for (DataIterator dit = dblTwo.dataIterator(); dit.ok(); ++dit)        ldIVSTwo[dit()] = IntVectSet(dblTwo.get(dit()));      BaseIVFactory<Real> factOne(ebislOne, ldIVSOne);      BaseIVFactory<Real> factTwo(ebislTwo, ldIVSTwo);      LevelData<BaseIVFAB<Real> > dataOne(dblOne, nvar, ivgh, factOne);      LevelData<BaseIVFAB<Real> > dataTwo(dblTwo, nvar, ivgh, factTwo);      for (DataIterator dit = dblOne.dataIterator(); dit.ok(); ++dit)        dataOne[dit()].setVal(1);      for (DataIterator dit = dblTwo.dataIterator(); dit.ok(); ++dit)        dataTwo[dit()].setVal(2);      //copy dataone into datatwo.  then all datatwo should hold are ones      dataOne.copyTo(interv, dataTwo, interv);      Real rightVal = 1.0;      for (DataIterator dit = dblTwo.dataIterator(); dit.ok(); ++dit)        {          const EBISBox& ebisBox = ebislTwo[dit()];          const IntVectSet&  ivs = ldIVSTwo[dit()];          const BaseIVFAB<Real>& fab = dataTwo[dit()];          for (VoFIterator vofit(ivs, ebisBox.getEBGraph()); vofit.ok(); ++vofit)            {              Real thisVal = fab(vofit(), 0);              if (Abs(thisVal - rightVal) > tolerance)                {                  eekflag = 3;                  return eekflag;                }            }        }      levelDomain.coarsen(2);    }  return eekflag;}