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 float
calc_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>::type
HermitianFrobeniusNorm
( 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