Example #1
0
static int
is_edge(double *intensity, struct application *ap, const struct cell *here,
	const struct cell *left, const struct cell *below, const struct cell *right, const struct cell *above)
{
    int found_edge = 0;

    if (!here) {
	return 0;
    }

    if (here && here->c_ishit) {
	if (detect_ids) {
	    if (left && below) {
		if (here->c_id != left->c_id || here->c_id != below->c_id) {
		    found_edge = 1;
		}
	    }
	    if (both_sides && right && above) {
		if (here->c_id != right->c_id || here->c_id != above->c_id) {
		    found_edge = 1;
		}
	    }
	}

	if (detect_regions) {
	    if (left && below) {
		if (here->c_region != left->c_region ||here->c_region != below->c_region) {
		    found_edge = 1;
		}
	    }
	    if (both_sides && right && above) {
		if (here->c_region != right->c_region || here->c_region != above->c_region) {
		    found_edge = 1;
		}
	    }
	}

	if (detect_distance) {
	    if (left && below) {
		if (Abs(here->c_dist - left->c_dist) > max_dist || Abs(here->c_dist - below->c_dist) > max_dist) {
		    found_edge = 1;
		}
	    }
	    if (both_sides && right && above) {
		if (Abs(here->c_dist - right->c_dist) > max_dist || Abs(here->c_dist - above->c_dist) > max_dist) {
		    found_edge = 1;
		}
	    }
	}

	if (detect_normals) {
	    if (left && below) {
		if ((VDOT(here->c_normal, left->c_normal) < COSTOL) || (VDOT(here->c_normal, below->c_normal)< COSTOL)) {
		    found_edge = 1;
		}
	    }
	    if (both_sides && right && above) {
		if ((VDOT(here->c_normal, right->c_normal)< COSTOL) || (VDOT(here->c_normal, above->c_normal)< COSTOL)) {
		    found_edge = 1;
		}
	    }
	}
    } else {
	if (left && below) {
	    if (left->c_ishit || below->c_ishit) {
		found_edge = 1;
	    }
	}
	if (both_sides && right && above) {
	    if (right->c_ishit || above->c_ishit) {
		found_edge = 1;
	    }
	}
    }

    if (found_edge) {
	if (intensity && ap && antialias)
	    get_intensity(intensity, ap, here, left, below, right, above);
	return 1;
    }
    return 0;
}
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);
}
Example #3
0
void Text::SetEffectStrokeThickness(int thickness)
{
    strokeThickness_ = Abs(thickness);
}
Example #4
0
 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);
       /* Radial vector */
       CVector3 cRadial(cCylBase2RayStart);
       cRadial -= fProj * m_cAxis;
       Real fDist = cRadial.Length();
       /* Is ray within the cylinder radius? */
       if(fDist > m_fRadius) {
          /* No, it's not */
          return false;
       }
       /* If we get here, it's because the ray might intersect the cylinder bases */
       Real fDenominator = cRayDir.DotProduct(m_cAxis) * fRayLen;
       /* Create a buffer for the 2 potential intersection points */
       Real fPotentialT[2];
       /* Bottom base */
       fPotentialT[0] =
          (m_cBasePos-c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
       /* Top base */
       fPotentialT[1] =
          (m_cBasePos + m_fHeight * m_cAxis - c_ray.GetStart()).DotProduct(m_cAxis) / fDenominator;
       /* Make sure these t's are within the cylinder surface */
       CVector3 cPoint;
       c_ray.GetPoint(cPoint, fPotentialT[0]);
       CVector3 cDiff = cPoint - m_cBasePos;
       if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
          fPotentialT[0] = -1;
       c_ray.GetPoint(cPoint, fPotentialT[1]);
       cDiff = cPoint - m_cBasePos;
       if((cDiff - cDiff.DotProduct(m_cAxis) * m_cAxis).SquareLength() > Square(m_fRadius))
          fPotentialT[1] = -1;
       /* Go through all the potential t's and get the best */
       f_t_on_ray = 2.0;
       for(UInt32 i = 0; i < 2; ++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);
    }
 }
Example #5
0
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;
}
Example #6
0
      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
Example #7
0
//
//#############################################################################
//#############################################################################
//
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
	//--------------------------------------------------
	//
	else
	{
		axis.Cross(start, end);
		delta.sine = axis.GetLength();
		axis /= delta.sine;

		//
		//---------------------------------------------------------------
		// Now compute sine and cosine of half the angle and generate the
		// quaternion
		//---------------------------------------------------------------
		//
		delta.sine = Sqrt((1.0f - delta.cosine)*0.5f);
		x = axis.x * delta.sine;
		y = axis.y * delta.sine;
		z = axis.z * delta.sine;
		w = Sqrt((1.0f + delta.cosine)*0.5f);
	}
	return *this;
}
Example #8
0
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);
}
Example #9
0
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;
}
Example #10
0
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
		//if( nd >= 0.0f )
		//{
		//	// Segment pointing away from endcap
		//	return false;
		//}
		//float t2 = ( dd - md ) / nd;
		//if( k + dd - 2.0f * md + t2 * ( 2.0f * ( mn - nd ) + t2 * nn ) > 0.0f )
		//{
		//	return false;
		//}
	}

	if( pInfo )
	{
		Vector Intersection = s.m_Point1 + t * n;
		Vector PointOnLine = Line( m_Point1, m_Point2 - m_Point1 ).NearestPointTo( 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;
}
Example #11
0
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;
}
Example #12
0
File: OBB.cpp Project: Ilikia/naali
/** 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;
}
Example #13
0
File: rc.c Project: gke/UAVXPIC
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
Example #14
0
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.
            const F eta = lambdaSub/lambda_ii;
            const F delta_i = lambda_ii;
            const F delta_ip1 = F(1) - eta*gamma;

            Axpy( -eta, lBi, lBip1 );
            A(i+1,i) = gamma/delta_i;
            lBi   *= F(1)/delta_i;
            lBip1 *= F(1)/delta_ip1;

            A(i,i) = eta*ups_ii*delta_i;
            Axpy( eta, uip1R, uiR );
            uiR   *= delta_i;
            uip1R *= delta_ip1;
            uSub(i) = ups_ii*delta_ip1;

            // Finally set w(i)
            w(i) = omega_ip1*delta_i;
        }
        else
        {
            // Update
            //     L := L T_{i,L}^{-1},
            //     U := T_{i,L} U, 
            //     w := T_{i,L} w,
            // where T_{i,L} is the Gauss transform which zeros w_{i+1}.
            // 
            // More succinctly,
            //     gamma    := w(i+1) / w(i),
            //     L(:,i)   += gamma L(:,i+1),
            //     U(i+1,:) -= gamma U(i,:),
            //     w(i+1)   := 0.
            const F gamma = omega_ip1 / omega_i;
            A(i+1,i) += gamma;
            Axpy(  gamma, lBip1, lBi );
            Axpy( -gamma, uiR, uip1R );
            uSub(i) = -gamma*ups_ii;
        }
    }

    // Add the modified w v' into U
    {
        auto a0 = A( IR(0), ALL );
        const F omega_0 = w(0); 
        Matrix<F> vTrans;
        Transpose( v, vTrans, conjugate );
        Axpy( omega_0, vTrans, a0 );
    }

    // Transform U from upper-Hessenberg to upper-triangular form
    for( Int i=0; i<minDim-1; ++i ) 
    {
        // Decide if we should pivot the i'th and i+1'th rows U
        const F lambdaSub = A(i+1,i);
        const F ups_ii = A(i,i);
        const F ups_ip1i = uSub(i);
        const Real rightTerm = Abs(lambdaSub*ups_ii+ups_ip1i);
        const bool pivot = ( Abs(ups_ii) < 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, 
            // where T_{i,L} is the Gauss transform which zeros U(i+1,i).
            // 
            // More succinctly,
            //     gamma    := U(i+1,i) / U(i,i),
            //     L(:,i)   += gamma L(:,i+1),
            //     U(i+1,:) -= gamma U(i,:).
            const F gamma = ups_ii / ups_ip1i;
            const F lambda_ii = F(1) + gamma*lambdaSub;
            A(i+1,i) = ups_ip1i;
            A(i,  i) = gamma;

            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}.
            const F eta = lambdaSub/lambda_ii;
            const F delta_i = lambda_ii;
            const F delta_ip1 = F(1) - eta*gamma;

            Axpy( -eta, lBi, lBip1 );
            A(i+1,i) = gamma/delta_i;
            lBi   *= F(1)/delta_i;
            lBip1 *= F(1)/delta_ip1;

            A(i,i) = ups_ip1i*delta_i;
            Axpy( eta, uip1R, uiR );
            uiR   *= delta_i;
            uip1R *= delta_ip1;
        }
        else
        {
            // Update
            //     L := L T_{i,L}^{-1},
            //     U := T_{i,L} U, 
            // where T_{i,L} is the Gauss transform which zeros U(i+1,i).
            // 
            // More succinctly,
            //     gamma    := U(i+1,i)/ U(i,i),
            //     L(:,i)   += gamma L(:,i+1),
            //     U(i+1,:) -= gamma U(i,:).
            const F gamma = ups_ip1i / ups_ii;
            A(i+1,i) += gamma;
            Axpy(  gamma, lBip1, lBi );
            Axpy( -gamma, uiR, uip1R );
        }
    }
}
Example #15
0
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
        R relScale = localScale/scale;
        localScaledSquare *= relScale*relScale;

        // The scaled square is now simply the sum of the local contributions
        R scaledSquare;
        mpi::AllReduce
        ( &localScaledSquare, &scaledSquare, 1, mpi::SUM, A.Grid().VCComm() );

        norm = scale*Sqrt(scaledSquare);
    }
#ifndef RELEASE
    PopCallStack();
#endif
    return norm;
}
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;
}
Example #17
0
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;
}
Example #18
0
/** 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;
}
Example #19
0
   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 )
Example #20
0
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 );
}
Example #21
0
 static bool Compare
 ( const IndexValuePair<R>& a, const IndexValuePair<R>& b )
 { return Abs(a.value) < Abs(b.value); }
Example #22
0
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;
}
Example #23
0
/*
** The GiST PickSplit method for _intments
** We use Guttman's poly time split algorithm
*/
Datum
g_int_picksplit(PG_FUNCTION_ARGS)
{
	GistEntryVector *entryvec = (GistEntryVector *) PG_GETARG_POINTER(0);
	GIST_SPLITVEC *v = (GIST_SPLITVEC *) PG_GETARG_POINTER(1);
	OffsetNumber i,
				j;
	ArrayType  *datum_alpha,
			   *datum_beta;
	ArrayType  *datum_l,
			   *datum_r;
	ArrayType  *union_d,
			   *union_dl,
			   *union_dr;
	ArrayType  *inter_d;
	bool		firsttime;
	float		size_alpha,
				size_beta,
				size_union,
				size_inter;
	float		size_waste,
				waste;
	float		size_l,
				size_r;
	int			nbytes;
	OffsetNumber seed_1 = 0,
				seed_2 = 0;
	OffsetNumber *left,
			   *right;
	OffsetNumber maxoff;
	SPLITCOST  *costvector;

#ifdef GIST_DEBUG
	elog(DEBUG3, "--------picksplit %d", entryvec->n);
#endif

	maxoff = entryvec->n - 2;
	nbytes = (maxoff + 2) * sizeof(OffsetNumber);
	v->spl_left = (OffsetNumber *) palloc(nbytes);
	v->spl_right = (OffsetNumber *) palloc(nbytes);

	firsttime = true;
	waste = 0.0;
	for (i = FirstOffsetNumber; i < maxoff; i = OffsetNumberNext(i))
	{
		datum_alpha = GETENTRY(entryvec, i);
		for (j = OffsetNumberNext(i); j <= maxoff; j = OffsetNumberNext(j))
		{
			datum_beta = GETENTRY(entryvec, j);

			/* compute the wasted space by unioning these guys */
			/* size_waste = size_union - size_inter; */
			union_d = inner_int_union(datum_alpha, datum_beta);
			rt__int_size(union_d, &size_union);
			inter_d = inner_int_inter(datum_alpha, datum_beta);
			rt__int_size(inter_d, &size_inter);
			size_waste = size_union - size_inter;

			pfree(union_d);
			pfree(inter_d);

			/*
			 * are these a more promising split that what we've already seen?
			 */

			if (size_waste > waste || firsttime)
			{
				waste = size_waste;
				seed_1 = i;
				seed_2 = j;
				firsttime = false;
			}
		}
	}

	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;
		}
		else if (i == seed_2)
		{
			*right++ = i;
			v->spl_nright++;
			continue;
		}

		/* okay, which page needs least enlargement? */
		datum_alpha = GETENTRY(entryvec, i);
		union_dl = inner_int_union(datum_l, datum_alpha);
		union_dr = inner_int_union(datum_r, datum_alpha);
		rt__int_size(union_dl, &size_alpha);
		rt__int_size(union_dr, &size_beta);

		/* pick which page to add it to */
		if (size_alpha - size_l < size_beta - size_r + WISH_F(v->spl_nleft, v->spl_nright, 0.01))
		{
			pfree(datum_l);
			pfree(union_dr);
			datum_l = union_dl;
			size_l = size_alpha;
			*left++ = i;
			v->spl_nleft++;
		}
		else
		{
			pfree(datum_r);
			pfree(union_dl);
			datum_r = union_dr;
			size_r = size_beta;
			*right++ = i;
			v->spl_nright++;
		}
	}
	pfree(costvector);
	*right = *left = FirstOffsetNumber;

	v->spl_ldatum = PointerGetDatum(datum_l);
	v->spl_rdatum = PointerGetDatum(datum_r);

	PG_RETURN_POINTER(v);
}
Example #24
0
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;
}
Example #26
0
inline BOOL EpsilonNe(const Type &a, const Type &b) { return Abs(a-b)> BSP_EPSILON; };
Example #27
0
/*
 * 函数名: rt_thread_entry_handle
 * 描  述: 手柄线程的入口函数
 * 输  入: 无
 * 输  出: 无
 * 调  用: 内部调用
 * 说  明: 
 */
static void rt_thread_entry_handle(void* parameter)
{
	double Handle_Speed_X;
	double Handle_Speed_Y;
	double Handle_Speed_Rotation;
	
	struct Point temp_point;
	struct Point now_point;
	u32 temp,temp2;
	u8 delay_flag = 1;
	u8 hall_flag = 0;
	u8 time_flag = 1,time_flag2 = 1;
	u8 handleoff_flag = 1,handleoff_flag2 = 1;
	u8 flag[6] = {0}; //对应电机的运行状态
	int16_t Speed_M = 500;
	float pos_openfan = 4;
	extern float Motor2_Aim_Pos[];
	extern u8 Motor2_Time_Flag;
	
	u8 x_con_flag = 0;	//X方向闭环
	u8 XY_Only = 0;
	Handle_Speed_X = 0;
	Handle_Speed_Y = 0;
  HandleData_X.lr= 12288;
  HandleData_Y.lr= 12288;
	TIM2->CNT = 0;
	
    while(1)
    {
		temp = *handle_count;
		temp2 = *handle_count2;
		while(*handle_count == temp)
		{
			time_flag++;
			Delay_ms(5);
			if(time_flag > 60)//超时
			{
				time_flag = 0;
				handle_timeout_flag = 1;
				if(handleoff_flag)
				{
					Handle_Off_Count++;
					handleoff_flag = 0;
				}
				break;
			}
		}
				while(*handle_count2 == temp2)
		{
			time_flag2++;
			Delay_ms(5);
			if(time_flag2 > 60)//超时
			{
				time_flag2 = 0;
				handle_timeout_flag2 = 1;
				if(handleoff_flag2)
				{
					Handle_Off_Count2++;
					handleoff_flag2 = 0;
				}
				break;
			}
		}
		
		if(time_flag||time_flag2)//手柄工作正常
		{
			time_flag = 1;
			time_flag2 = 1;
			handle_timeout_flag = 0;
			handle_timeout_flag2 = 0;
			handleoff_flag = 1;
			handleoff_flag2 = 1;
			Handle_Speed_X = HandleData_X.lr-12288;
			Handle_Speed_Y = HandleData_Y.lr-12288;
			Handle_Speed_Rotation = 0;
			//Handle_Speed_Y = HandleData_Y.ud-12288;
			//Handle_Speed_Rotation = (HandleData_X.turn-12288)/10.0;
			
			if(Button10_On && Button7_Off)
			{
				Handle_Speed_X /= 10.0;
				Handle_Speed_Y /= 10.0;
				Handle_Speed_Rotation /= 10.0;
				Speed_M = 150;
			}
			else
				Speed_M = 500;
			
			if(Button9_On)
			{
//				HandoverFlag[0]=1;
//				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)
			{
				flag[2]=1;
			    Moto_Set_GSpeed(W_MOTOR2_OLD_ID, 3*Speed_M);
			}
			if(Button2_Off && (flag[2]==1))
			{
				Moto_Stop(W_MOTOR2_OLD_ID);
				flag[2]=0;
			}
			
			
			if(Button3_Up && Button7_Off && flag[3]==0)
			{
				flag[3]=1;
			    Moto_Set_GSpeed(W_MOTOR3_OLD_ID, -30*Speed_M);
			}
			if(Button3_Down && Button7_Off && flag[3]==0)
			{
				flag[3]=1;
			    Moto_Set_GSpeed(W_MOTOR3_OLD_ID, 30*Speed_M);
			}
			if(Button3_Off && (flag[3]==1))
			{
				flag[3]=0;
				Moto_Stop(W_MOTOR3_OLD_ID);
			}
			
			//------------涵道------------//
			if(Button4_Up && Button7_Off && Button10_Off)
			{
			    Fan_Duty(L_CH,60.0);
				Fan_Duty(R_CH,60.0);
			}
			if(Button4_Down && Button7_Off && Button10_Off)
			{
			    Fan_Stop();
			}
			
			//------------霍尔------------//
			if(Button5_Up && Button7_Off && Button10_Off && hall_flag == 0)
			{
				hall_flag = 1;
				if(Hall_Count > 6) Hall_Count=1;
			    Hall_Send(Hall_Count);
			}
			if(Button5_Down && Button7_Off && Button10_Off && hall_flag == 0)
			{
				hall_flag = 1;
				if(Hall_Count > 1) Hall_Count -= 2;
				Hall_Send(Hall_Count);
			}
			if(Button5_Off && hall_flag == 1)
			{
				hall_flag = 0;
			}
			
		  	//------------双键组合用来控制电磁阀,7键为第二功能键------------//
			
			if(Button6_Up && Button7_Off && Button10_Off){
				HandPush();
			}
			if(Button6_Down && Button7_Off && Button10_Off){
				HandPushOff();
			}
			
			//缓冲装置
			if(Button1_Up && Button7_On && Button10_Off){
				BufferOn();
			}
			if(Button1_Down && Button7_On && Button10_Off){
				BufferOff();
			}
			//旋转调姿
		  	if(Button2_Up && Button7_On && Button10_Off){
				HandTurnRight();
			}
			if(Button2_Down && Button7_On && Button10_Off){
				HandTurnLeft();
			}
			//俯仰调姿
			if(Button3_Up && Button7_On && Button10_Off){
				HandRaise();
			}
			if(Button3_Down && Button7_On && Button10_Off){
				HandFall();
			}
			//放置大炮
			if(Button4_Up && Button7_On && Button10_Off){
			    GunOn();
			}
			
			if(Button4_Down && Button7_On && Button10_Off){
			    GunOff();
			}
			//开炮
     		if(Button5_Up && Button7_On && Button10_Off){
				FireOn();
			}
			
			if(Button5_Down && Button7_On && Button10_Off){
				FireOff();
			}
			
			
			//交接爪
			if(Button6_Up && Button7_On && Button10_Off){
			    HandOpen();
			}
			
			if(Button6_Down && Button7_On && Button10_Off){
			    HandClose();
			}
			
			//------------双键组合用来控制上下电机的位置,10键为第二功能键------------//
			
			if(Button4_Up && Button7_Off && Button10_On){
			    Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[0]);
			}
			
			if(Button4_Down && Button7_Off && Button10_On){
			    Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[2]);
			}
			if(Button5_Up && Button7_Off && Button10_On){
			    Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[4]);
			}
			
			if(Button5_Down && Button7_Off && Button10_On){
			    Moto_Set_GPos(W_MOTOR2_OLD_ID,Motor2_Aim_Pos[6]);
			}
			
			if(Button6_Up && Button7_Off && Button10_On){
			    HandUD(0,NO);
				Delay_ms(500);
				HandClose();//关爪子
				HandFB(0,NO,3500);
				FireOn();//开炮
				Delay_ms(100);

				now_point.x = GPS.position.x-700*cos(GPS.radian);
				now_point.y = GPS.position.y-700*sin(GPS.radian);
				SetLine(now_point,GPS.radian,100,1200,0);
				
				while(1)
				{
					GoLine();
					SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);
					if(delay_flag)
						delay_flag++;
					if(delay_flag>100)
					{
						delay_flag = 0;
						FireOff();
					}
					if(GetLength(GPS.position,now_point) < 10)
					{
						SPEED_STOP;
						break;
					}
					Delay_ms(5);
				}
				delay_flag = 1;
				FireClear();
				
				Delay_ms(50);
				BufferOn();//打开缓冲装置
				Delay_ms(500);
				BufferOff();//重新复位缓冲装置
			}
			
			if(Button6_Down && Button7_Off && Button10_On){
			    now_point.x = GPS.position.x+680*cos(GPS.radian);
				now_point.y = GPS.position.y+680*sin(GPS.radian);
				SetLine(now_point,GPS.radian,300,1200,0);
				HandFB(Motor1_Aim_Pos[4],NO,3500);//抓取自动机器人
				while(1)
				{
					GoLine();
					SetSpeed(Speed_X+Handle_Speed_X,Speed_Y+Handle_Speed_Y,Speed_Rotation+Handle_Speed_Rotation);
					if(GetLength(GPS.position,now_point) < 10)
					{
						SPEED_STOP;
						break;
					}
					Delay_ms(5);
				}
				SetSpeed(Speed_X,Speed_Y,Speed_Rotation);
				
				HandUD(0.5,NO);
				HandOpen();//松开爪子
				
				Delay_ms(300);
				FireOff();
				BufferOn();//打开缓冲装置
			}
			
			
			//三键组合用来重复任务
			if(Button1_Up && Button7_On && Button10_On){
				Retry = 1;
				RouteFinish=0;
				Hall_Send(1);//翘翘板初始化
				rt_mb_send(&Mb_Emergency, 7);
				rt_mb_send (&Mb_Arm, 1);
			}
			if(Button2_Up && Button7_On && Button10_On){
				Retry = 1;
				RouteFinish=0;
				Hall_Send(2);//梅花桩初始化
				rt_mb_send(&Mb_Emergency, 7);
				rt_mb_send (&Mb_Arm, 4);
			}
			if(Button3_Up && Button7_On && Button10_On){
				Retry = 1;
				RouteFinish=0;
				Hall_Send(4);//秋千初始化
				rt_mb_send(&Mb_Emergency, 7);
				rt_mb_send (&Mb_Arm, 6);
			}
			if(Button4_Up && Button7_On && Button10_On){
				Retry = 1;
				RouteFinish=0;
				Hall_Send(6);//楼梯初始化
				rt_mb_send(&Mb_Emergency, 7);
				rt_mb_send (&Mb_Arm, 7);
			}
			if(Button5_Up && Button7_On && Button10_On){
					rt_mb_send(&Mb_Emergency, 6);
			}
			if(Button6_Up && Button7_On && Button10_On){
				Retry = 1;
				RouteFinish=0;
				rt_mb_send(&Mb_Emergency, 7);
				//Motor_Init();
				FireOff();
				BufferOn();
				ChooseGround_Pole(GROUND);
				W_MOTOR_OLD_FUNC(W_MOTOR1_OLD_ID, 0.5 , 1000 , CMD_SET_PSG);
				Moto_Set_GPos(W_MOTOR2_OLD_ID,2.55);
				LCD_Clear();
				LCD_SetXY(0,1);
				LCD_Printf("Ready to go!");
				Wait_Button8_Change();
				rt_mb_send (&Mb_Auto, 1);
			}
		}
		
		else
		{
			SPEED_STOP;
			//Acc_Limit_Enable = 0;
			SetSpeed(0,0,0);
			//Acc_Limit_Enable = 1;
			Moto_Stop(W_MOTOR1_OLD_ID);
			Moto_Stop(W_MOTOR2_OLD_ID);
			//Moto_Stop(W_MOTOR3_OLD_ID);
			//Com_Printf(5,"v0\r");
		}
        
		if(Abs(Motor_Pos[0])>pos_openfan && CHILD_ON && !fan_flag)
		{
			fan_flag = 1;
			Fan_Duty(L_CH,60.0);
			Fan_Duty(R_CH,60.0);
		}
		if((Abs(Motor_Pos[0])<pos_openfan || CHILD_OFF) && fan_flag)
		{
			fan_flag = 0;
			Fan_Stop();
		}
		
		if(Motor2_Time_Flag>10)
		{
			//W_MOTOR_OLD_FUNC(W_MOTOR2_OLD_ID, 0 , 0 , CMD_INIT_CAN);
			Motor2_Time_Flag = 0;
		}
		
        Delay_ms(10);
    }
}
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;
	}
Example #29
0
/** 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();
	init_xosc32k_fail_detector(
			xosc32k_ok_callback, xosc32k_fail_callback);

#if ENABLE_CPU_CLOCK_OUT == true
	/* Configure a GPIO pin as the CPU clock output */
	struct system_pinmux_config clk_out_pin;
	system_pinmux_get_config_defaults(&clk_out_pin);
	clk_out_pin.direction    = SYSTEM_PINMUX_PIN_DIR_OUTPUT;
	clk_out_pin.mux_position = CONF_CLOCK_PIN_MUX;
	system_pinmux_pin_set_config(CONF_CLOCK_PIN_OUT, &clk_out_pin);
#endif

	for (;;) {
		static bool old_run_osc = true;
		bool new_run_osc =
				(port_pin_get_input_level(BUTTON_0_PIN) == BUTTON_0_INACTIVE);

		/* Check if the XOSC32K needs to be started or stopped when the board
		 * button is pressed or released */
		if (new_run_osc != old_run_osc) {
			if (new_run_osc) {
				system_clock_source_enable(SYSTEM_CLOCK_SOURCE_XOSC32K);
				while(!system_clock_source_is_ready(
						SYSTEM_CLOCK_SOURCE_XOSC32K));
			}
			else {
				system_clock_source_disable(SYSTEM_CLOCK_SOURCE_XOSC32K);
			}

			old_run_osc = new_run_osc;
		}
	}
}
Example #30
0
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;
}