Example #1
0
/*****************************************************************************
 * Function: _DtCvDrawAreaWithFlags
 *
 *****************************************************************************/
void
_DtCvDrawAreaWithFlags (
    _DtCanvasStruct	*canvas,
    _DtCvSelectData	 start,
    _DtCvSelectData	 end,
    _DtCvFlags		 old_flags,
    _DtCvFlags		 new_flags,
    _DtCvElemType	 trav_type,
    _DtCvPointer	 trav_data)
{
    int    i;
    int    len;
    int    count;
    int    startChar;
    int    lnkInd;
    _DtCvUnit   dstX;
    _DtCvUnit   topY;
    _DtCvUnit   botY;
    _DtCvUnit   superWidth;
    _DtCvUnit   subWidth;
    _DtCvUnit   superY;
    _DtCvUnit   subY;
    _DtCvUnit   scriptX;
    _DtCvUnit   segWidth;
    _DtCvSegmentI  *pSeg;
    _DtCvValue   lstLinkVis;
    _DtCvValue   lstWasSuper;
    _DtCvValue   lstWasSub;
    _DtCvValue   trimmed;
    _DtCvFlags	 flagMask = old_flags | new_flags;
    _DtCvFlags	 endFlag  = flagMask & _DtCvTRAVERSAL_END;

    _DtCvDspLine   *lines = canvas->txt_lst;

    /*
     * now use the flagMask to determine what else to look for.
     * I.e. if flagMask has _DtCvMARK_FLAG set, then it becomes
     * set to _DtCvSELECTED_FLAG and visa versa.
     */
    flagMask ^= (_DtCvSELECTED_FLAG | _DtCvMARK_FLAG);

    /*
     * strip the end flag from the other flags
     */
    new_flags &= ~(_DtCvTRAVERSAL_END);
    old_flags &= ~(_DtCvTRAVERSAL_END);

    if (Equal(start, end))
	return;

    for (i = 0; i < canvas->txt_cnt; i++)
      {
	topY  = lines[i].baseline - lines[i].ascent;
	botY  = lines[i].baseline + lines[i].descent;

	if (InRegion(topY, botY, start.y, end.y))
	  {
	    /*
	     * get the start of the text.
	     */
	    lstLinkVis  = False;
	    lstWasSuper = False;
	    lstWasSub   = False;
	    lnkInd      = -1;
	    dstX        = _DtCvGetStartXOfLine(&(lines[i]), &pSeg);
	    startChar   = lines[i].byte_index;
	    count       = lines[i].length;

	    while (pSeg != NULL && _DtCvIsSegNoop(pSeg))
	      {
	        startChar  = 0;
	        pSeg       = pSeg->next_disp;
	      }

	    /*
	     * advance the starting point
	     */
	    dstX = _DtCvAdvanceXOfLine(canvas, pSeg, dstX,
					&lnkInd, &lstLinkVis);
	    /*
	     * take into account super/sub scripting
	     */
	    dstX = _DtCvAdjustForSuperSub(canvas, pSeg, dstX, &scriptX,
					&superWidth, &superY, &subWidth, &subY,
					&lstWasSuper, &lstWasSub);

	    /*
	     * set this flag so that the first pass of 'while (cnt > 0)'
	     * doesn't do it again.
	     */
	    trimmed = True;

	    if (_DtCvStraddlesPt(start.y, topY, botY))
	      {
		/*
		 * skip this item?
		 * I.E. is this line before the start or after the end?
		 */
		if (canvas->txt_lst[i].max_x < start.x ||
					end.y == start.y && end.x <= dstX )
		    continue;

		/*
		 * does this line start the mark/selection?
		 */
		if (i == start.line_idx && start.x >= dstX)
		  {
		    int cnt = start.char_idx;

		    while (cnt > 0)
		      {
			if (trimmed == False)
			  {
			    /*
			     * advance the starting point
			     */
			    dstX = _DtCvAdvanceXOfLine(canvas, pSeg, dstX,
					&lnkInd, &lstLinkVis);
			    /*
			     * take into account super/sub scripting
			     */
			    dstX = _DtCvAdjustForSuperSub(canvas,
					pSeg, dstX, &scriptX,
					&superWidth, &superY, &subWidth, &subY,
					&lstWasSuper, &lstWasSub);
			  }

			/*
			 * take into account the length of the segment
			 */
			_DtCvGetWidthOfSegment(canvas, pSeg,
					startChar , cnt,
					&len, &segWidth, &trimmed);

			dstX       += segWidth;
			startChar  += len;
			if (trimmed == False)
			  {
			    startChar  = 0;
			    pSeg       = pSeg->next_disp;
			  }

			trimmed = False;

			cnt -= len;

		      }

		    count -= start.char_idx;
		  }

		/*
		 * otherwise this line is after the line that starts
		 * the mark/selection. Stick with its start x.
		 */
	      }

	    /*
	     * does this straddle the end point?
	     */
	    if (_DtCvStraddlesPt(end.y, topY, botY))
	      {
		/*
		 * does this start after the end of the mark/selection?
		 * if so, skip.
		 */
		if (end.x <= dstX)
		    continue;

		/*
		 * Does this segment end after the end of the mark/selection?
		 * If so, trim how much gets highlighted.
		 */
		if (canvas->txt_lst[i].max_x > end.x)
		    count -= (lines[i].length - end.char_idx);
	      }

	    /*
	     * while there is something to draw (un)mark/selected.
	     */
	    old_flags = old_flags | _DtCvMARK_BEGIN;
	    new_flags = new_flags | _DtCvMARK_BEGIN;
	    while (count > 0)
	      {
		/*
		 * the original count to render
		 */
		len = count;

		/*
		 * check for other marks and selection.
		 */
		_DtCvCheckLineMarks(canvas, i, startChar - lines[i].byte_index,
						count, dstX, flagMask,
						&len, &old_flags, &new_flags);

		/*
		 * if this is the last segment(s) of the (un)mark/selection
		 * set the end flags.
		 */
		if (len == count)
		  {
		    new_flags |= (endFlag | _DtCvLINK_END | _DtCvMARK_END);
		    old_flags |= (endFlag | _DtCvLINK_END | _DtCvMARK_END);
		  }

		/*
		 * draw the segments that are marked/unmarked.
		 */
	        dstX = _DtCvDrawSegments(canvas, lines[i],
					pSeg, startChar , len, &lnkInd,
					dstX, dstX, &scriptX,
					&superWidth, &superY, &subWidth, &subY,
					&lstWasSub, &lstWasSuper,
					&lstLinkVis, old_flags, new_flags,
					trav_type, trav_data);
		/*
		 * modify the count by the length processed
		 */
		count  -= len;

		/*
		 * did this do the entire length? If not, set the
		 * indexes ahead and do again.
		 */
		if (count > 0)
		    _DtCvSkipLineChars(canvas, pSeg, startChar , count + len,
						len, &startChar , &pSeg);

		/*
		 * strip the any begin flags.
		 */
		_DtCvRemoveBeginFlags(old_flags);
		_DtCvRemoveBeginFlags(new_flags);
	      }
	  }
      }
}
Example #2
0
 /**
  * Constructor.
  * @param n initial table size.
  * @param hash hash function.
  * @param equal equality function
  */
 MyHashTable(size_t n, Hash const& hash = Hash(), Equal const& equal =
         Equal())
         : hashFunc(hash), eqFunc(equal), tableCapacity_(0), tableSize_(0),
           maxSize_(0), size_(0), table(0), collisions_(0) {
     initialize(n);
 }
Example #3
0
 /**
  * Constructor.
  * @param n initial table size.
  * @param hash hash function.
  * @param equal equality function.
  */
 MyHashMap(size_t n, Hash const& hash = Hash(), Equal const& equal = Equal())
         : Table(n, hash, equal) {
 }
Example #4
0
  VOID tEDGE_RING<_Mesh_Type, _Half_Edge_Filter>::FindPrevAndNext(INT ci, INT &pi, INT &ni)
   {
     INT i = 0;
     INT nIndex;

     for( ; i < numEntries ; i++) {
       nIndex = (ci + i + 1) % numEntries;
       if(!Equal(entries[nIndex].angle,
		 entries[ci].angle))
	 break;
     }
     
     if(numEntries == i)
       ni = (ci + 1)%numEntries;
     else
       ni = nIndex;
     
     INT pIndex;
     i = 0;
     for( ; i < numEntries ; i++) {
       pIndex = (numEntries + ci - ( i + 1)) % numEntries;
       if(!Equal(entries[pIndex].angle,
		 entries[ci].angle))
	 break;
     }
     
     if(numEntries == i)
       pi = (numEntries + ci - 1)%numEntries;
     else
       pi = pIndex;
     
     
     //In case of overlapping geometry there is a possibility to find a matching 
     //half edge for prev with the same radial position.
     if(!AreOpposite(entries[pi].he,
		     entries[ci].he)) {
       i = 0;
       for( ; i < numEntries ; i++) {
	 pIndex = (numEntries + pi - ( i + 1)) % numEntries;
	 
	 if(pIndex == ni)
	   break;
	 
	 if(!Equal(entries[pIndex].angle,
		   entries[pi].angle))
	   break;
	 else {
	   if(AreOpposite(entries[pIndex].he,
			  entries[ci].he)) {
	     pi = pIndex;
	     break;
	   }
	 }
       }
     }

     //In case of overlapping geometry there is a possibility to find a matching 
     //half edge for next with the same radial position.
     if(!AreOpposite(entries[ni].he,
		     entries[ci].he)) {
       i = 0;
       for( ; i < numEntries ; i++) {
	 nIndex = (ni + i + 1) % numEntries;
	 
	 if(nIndex == pi)
	   break;
	 
	 if(!Equal(entries[ni].angle,
		   entries[nIndex].angle))
	   break;
	 else {
	   if(AreOpposite(entries[nIndex].he,
			  entries[ci].he)) {
	     ni = nIndex;
	     break;
	   }
	 }
       }
     }
     
     if(ni == pi) {
       ni = (ci + 1)%numEntries;
       pi = (numEntries+ci - 1)%numEntries;
     }
   }
Example #5
0
bool CDVDStreamInfo::Equal(const CDemuxStream& right, bool withextradata)
{
  CDVDStreamInfo info;
  info.Assign(right, withextradata);
  return Equal(info, withextradata);
}
Example #6
0
int checkAttribute(Node *n, const_String_or_char_ptr name, const_String_or_char_ptr value) {
  String *v = Getattr(n, name);
  return v ? Equal(v, value) : 0;
}
Example #7
0
File: sfcmp.c Project: 274914765/C
/*
 * sgl_cmp: compare two values
 */
int
sgl_fcmp (sgl_floating_point * leftptr, sgl_floating_point * rightptr,
          unsigned int cond, unsigned int *status)

/* The predicate to be tested */

{
    register unsigned int left, right;
    register int xorresult;

    /* Create local copies of the numbers */
    left = *leftptr;
    right = *rightptr;

    /*
     * Test for NaN
     */
    if(    (Sgl_exponent(left) == SGL_INFINITY_EXPONENT)
            || (Sgl_exponent(right) == SGL_INFINITY_EXPONENT) )
    {
        /* Check if a NaN is involved.  Signal an invalid exception when
         * comparing a signaling NaN or when comparing quiet NaNs and the
         * low bit of the condition is set */
        if( (  (Sgl_exponent(left) == SGL_INFINITY_EXPONENT)
                && Sgl_isnotzero_mantissa(left)
                && (Exception(cond) || Sgl_isone_signaling(left)))
                ||
                (  (Sgl_exponent(right) == SGL_INFINITY_EXPONENT)
                   && Sgl_isnotzero_mantissa(right)
                   && (Exception(cond) || Sgl_isone_signaling(right)) ) )
        {
            if( Is_invalidtrap_enabled() ) {
                Set_status_cbit(Unordered(cond));
                return(INVALIDEXCEPTION);
            }
            else Set_invalidflag();
            Set_status_cbit(Unordered(cond));
            return(NOEXCEPTION);
        }
        /* All the exceptional conditions are handled, now special case
           NaN compares */
        else if( ((Sgl_exponent(left) == SGL_INFINITY_EXPONENT)
                  && Sgl_isnotzero_mantissa(left))
                 ||
                 ((Sgl_exponent(right) == SGL_INFINITY_EXPONENT)
                  && Sgl_isnotzero_mantissa(right)) )
        {
            /* NaNs always compare unordered. */
            Set_status_cbit(Unordered(cond));
            return(NOEXCEPTION);
        }
        /* infinities will drop down to the normal compare mechanisms */
    }
    /* First compare for unequal signs => less or greater or
     * special equal case */
    Sgl_xortointp1(left,right,xorresult);
    if( xorresult < 0 )
    {
        /* left negative => less, left positive => greater.
         * equal is possible if both operands are zeros. */
        if( Sgl_iszero_exponentmantissa(left)
                && Sgl_iszero_exponentmantissa(right) )
        {
            Set_status_cbit(Equal(cond));
        }
        else if( Sgl_isone_sign(left) )
        {
            Set_status_cbit(Lessthan(cond));
        }
        else
        {
            Set_status_cbit(Greaterthan(cond));
        }
    }
    /* Signs are the same.  Treat negative numbers separately
     * from the positives because of the reversed sense.  */
    else if( Sgl_all(left) == Sgl_all(right) )
    {
        Set_status_cbit(Equal(cond));
    }
    else if( Sgl_iszero_sign(left) )
    {
        /* Positive compare */
        if( Sgl_all(left) < Sgl_all(right) )
        {
            Set_status_cbit(Lessthan(cond));
        }
        else
        {
            Set_status_cbit(Greaterthan(cond));
        }
    }
    else
    {
        /* Negative compare.  Signed or unsigned compares
         * both work the same.  That distinction is only
         * important when the sign bits differ. */
        if( Sgl_all(left) > Sgl_all(right) )
        {
            Set_status_cbit(Lessthan(cond));
        }
        else
        {
            Set_status_cbit(Greaterthan(cond));
        }
    }
    return(NOEXCEPTION);
}
Example #8
0
/*** Evaluate this object ***/
Object* Equal::evaluate()
{
	if(arg1 == 0)
		throw MissingArgumentException(getLineNumber(), getColumnNumber(), OBJ_INTEGER | OBJ_REAL | OBJ_SEQUENCE | OBJ_TEXT | OBJ_LOGICAL, 1);
	if(arg2 == 0)
		throw MissingArgumentException(getLineNumber(), getColumnNumber(), OBJ_INTEGER | OBJ_REAL | OBJ_SEQUENCE | OBJ_TEXT | OBJ_LOGICAL, 2);

	std::auto_ptr<Object> obj1(arg1->evaluate());
	std::auto_ptr<Object> obj2(arg2->evaluate());

	if(obj1->getType() == OBJ_EMPTY && obj2->getType() == OBJ_EMPTY)
		return new Logical(true);

	if(obj1->getType() == OBJ_EMPTY || obj2->getType() == OBJ_EMPTY)
		return new Logical(false);

	if(obj1->getType() == OBJ_LOGICAL)
	{
		if(obj2->getType() != OBJ_LOGICAL)
			throw InvalidTypeException(getLineNumber(), getColumnNumber(), OBJ_LOGICAL, obj2->getType(), 2);

		Logical* cast1 = static_cast<Logical*>(obj1.get());
		Logical* cast2 = static_cast<Logical*>(obj2.get());
		return new Logical(cast1->getValue() == cast2->getValue());
	}

	if(obj1->getType() == OBJ_INTEGER)
	{
		Integer* cast1 = static_cast<Integer*>(obj1.get());

		if(obj2->getType() == OBJ_INTEGER)
		{
			Integer* cast2 = static_cast<Integer*>(obj2.get());
			return new Logical(cast1->getValue() == cast2->getValue());
		}

		if(obj2->getType() == OBJ_REAL)
		{
			Real* cast2 = static_cast<Real*>(obj2.get());
			return new Logical(cast1->getValue() == cast2->getValue());
		}
		
		throw InvalidTypeException(getLineNumber(), getColumnNumber(), OBJ_INTEGER | OBJ_REAL, obj2->getType(), 2);
	}

	if(obj1->getType() == OBJ_REAL)
	{
		Real* cast1 = static_cast<Real*>(obj1.get());

		if(obj2->getType() == OBJ_INTEGER)
		{
			Integer* cast2 = static_cast<Integer*>(obj2.get());
			return new Logical(cast1->getValue() == cast2->getValue());
		}

		if(obj2->getType() == OBJ_REAL)
		{
			Real* cast2 = static_cast<Real*>(obj2.get());
			return new Logical(cast1->getValue() == cast2->getValue());
		}
		
		throw InvalidTypeException(getLineNumber(), getColumnNumber(), OBJ_INTEGER | OBJ_REAL, obj2->getType(), 2);
	}

	if(obj1->getType() == OBJ_TEXT)
	{
		if(obj2->getType() != OBJ_TEXT)
			throw InvalidTypeException(getLineNumber(), getColumnNumber(), OBJ_TEXT, obj2->getType(), 2);

		Text* cast1 = static_cast<Text*>(obj1.get());
		Text* cast2 = static_cast<Text*>(obj2.get());
		return new Logical(cast1->getValue().compare(cast2->getValue()) == 0);
	}

	if(obj1->getType() == OBJ_SEQUENCE)
	{
		if(obj2->getType() != OBJ_SEQUENCE)
			throw InvalidTypeException(getLineNumber(), getColumnNumber(), OBJ_SEQUENCE, obj2->getType(), 2);

		Sequence* cast1 = static_cast<Sequence*>(obj1.get());
		Sequence* cast2 = static_cast<Sequence*>(obj2.get());

		if(cast1->getLength() != cast2->getLength())
			return new Logical(false);

		for(unsigned int i = 0; i < cast1->getLength(); i++)
		{
			std::auto_ptr<Object> eqResult(Equal(cast1->getObject(i)->clone(), cast2->getObject(i)->clone()).evaluate());
			Logical* logResult = static_cast<Logical*>(eqResult.get());
			if(logResult->getValue() == false)
				return new Logical(false);
		}

		return new Logical(true);
	}

	throw InvalidTypeException(getLineNumber(), getColumnNumber(), OBJ_LOGICAL | OBJ_REAL | OBJ_TEXT | OBJ_SEQUENCE | OBJ_INTEGER, obj1->getType(), 1);
	return 0;
}
Example #9
0
inline bool operator != (const Vector<T, N>& a, const Vector<T, N>& b)
{
    return !Equal(a, b);
}
Example #10
0
void Manifold::ApplyImpulse( void )
{
  // Early out and positional correct if both objects have infinite mass
  if(Equal( A->im + B->im, 0 ))
  {
    InfiniteMassCorrection( );
    return;
  }

  for(uint32 i = 0; i < contact_count; ++i)
  {
    // Calculate radii from COM to contact
    Vec2 ra = contacts[i] - A->position;
    Vec2 rb = contacts[i] - B->position;

    // Relative velocity
    Vec2 rv = B->velocity + Cross( B->angularVelocity, rb ) -
              A->velocity - Cross( A->angularVelocity, ra );

    // Relative velocity along the normal
    real contactVel = Dot( rv, normal );

    // Do not resolve if velocities are separating
    if(contactVel > 0)
      return;

    real raCrossN = Cross( ra, normal );
    real rbCrossN = Cross( rb, normal );
    real invMassSum = A->im + B->im + Sqr( raCrossN ) * A->iI + Sqr( rbCrossN ) * B->iI;

    // Calculate impulse scalar
    real j = -(1.0f + e) * contactVel;
    j /= invMassSum;
    j /= (real)contact_count;

    // Apply impulse
    Vec2 impulse = normal * j;
    A->ApplyImpulse( -impulse, ra );
    B->ApplyImpulse(  impulse, rb );

    // Friction impulse
    rv = B->velocity + Cross( B->angularVelocity, rb ) -
         A->velocity - Cross( A->angularVelocity, ra );

    Vec2 t = rv - (normal * Dot( rv, normal ));
    t.Normalize( );

    // j tangent magnitude
    real jt = -Dot( rv, t );
    jt /= invMassSum;
    jt /= (real)contact_count;

    // Don't apply tiny friction impulses
    if(Equal( jt, 0.0f ))
      return;

    // Coulumb's law
    Vec2 tangentImpulse;
    if(std::abs( jt ) < j * sf)
      tangentImpulse = t * jt;
    else
      tangentImpulse = t * -j * df;

    // Apply friction impulse
    A->ApplyImpulse( -tangentImpulse, ra );
    B->ApplyImpulse(  tangentImpulse, rb );
  }
}
int main()
{
	Copyright();  //输出程序信息

	char *s = (char *)malloc(MAXLEN * sizeof(char));         //用来保存输入数据
	char *backups_s = s;                                     //备份s指针 用于释放内存
    char *filepath = (char *)malloc(MAXLEN * sizeof(char));  //用来储存文件路径
	char *backups_p = filepath;
	while (1)
	{
        Pre();
		s = backups_s;
		Read_Argv(s);
		if (*s != '-')
		{
			gets(s);
			Error();
			continue;
		}
		s ++;
		filepath = backups_p;
        
		if (Equal(s , "index"))
		{
		    int t;
			scanf("%d" , &t);
			if (t == 1)
			    printf("---Open the index function successfully.---\n");
			else
				if (t == 0)
					printf("---Close the index function successfully.---\n");
				else
					Error();
			gets(s);
			continue;
		}
		else
			if (*(s + 1) != '\0')
			{
				gets(s);
				Error();
				continue;
			}
		switch (*s) 
		{
		    case 'c' : Get_Path(filepath);
				       Create_Table(filepath);
					   break;

			case 'i' : s = backups_s;   
					   Get_Table(s);        //第二个参数 表名
				       Get_Path(filepath);
					   Import_Data(s , filepath);
					   break;

			case 's' : Get_Path(filepath);
					   Select(filepath);
					   break;

			case 'u' : Get_Path(filepath);
					   Update(filepath);
				       break;

			case 'd' : Get_Path(filepath);
					   Delete(filepath);
					   break;

			case 'h' : Help();
				       break;

		    case 'q' : Exit_pro();
					   free(backups_s);
					   free(backups_p);
					   Free();
				       return 0;

			default  : gets(s);
				       Error();
				       break;
		}
		gets(s);
	}

	return 0;
}
Example #12
0
 bool Equal(const JntArrayVel& src1,const JntArrayVel& src2,double eps)
 {
     return Equal(src1.q,src2.q,eps)&&Equal(src1.qdot,src2.qdot,eps);
 }
Example #13
0
static void TestAliasMultUL2(
    const M& m, const Ma& ma, const Mb& mb,
    M1& m1, const M1a& m1a, const M1b& m1b, std::string label)
{
    if (showstartdone) {
        std::cout<<"Start AliasMultUL2\n";
        std::cout<<"m = "<<m<<std::endl;
        std::cout<<"ma = "<<ma<<std::endl;
        std::cout<<"mb = "<<mb<<std::endl;
    }

    typedef typename M::value_type T;
    typedef typename M::real_type RT;
    const int N = m.colsize();
    tmv::Matrix<T> m2(N,N);

    RT eps = EPS * RT(N);
    if (!std::numeric_limits<RT>::is_integer) eps *= Norm(m);

    m1 = m;
    m1 = m1a * m1b;
    m2 = ma * mb;
    Assert(Equal(m1,m2,eps),label+" m=a*b");

    m1 = m;
    m1 += m1a * m1b;
    m2 = m + ma * mb;
    Assert(Equal(m1,m2,eps),label+" m+=a*b");

    m1 = m;
    m1 = T(10) * m1a * m1b;
    m2 = T(10) * ma * mb;
    Assert(Equal(m1,m2,eps),label+" m=10*a*b");

    m1 = m;
    m1 += T(10) * m1a * m1b;
    m2 = m + T(10) * ma * mb;
    Assert(Equal(m1,m2,eps),label+" m+=10*a*b");

    m1 = m;
    m1 = m1a * m1b.transpose();
    m2 = ma * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m=a*bT");

    m1 = m;
    m1 += m1a * m1b.transpose();
    m2 = m + ma * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m+=a*bT");

    m1 = m;
    m1 = T(10) * m1a * m1b.transpose();
    m2 = T(10) * ma * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m=10*a*bT");

    m1 = m;
    m1 += T(10) * m1a * m1b.transpose();
    m2 = m + T(10) * ma * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m+=10*a*bT");

    m1 = m;
    m1 = m1a.transpose() * m1b;
    m2 = ma.transpose() * mb;
    Assert(Equal(m1,m2,eps),label+" m=aT*b");

    m1 = m;
    m1 += m1a.transpose() * m1b;
    m2 = m + ma.transpose() * mb;
    Assert(Equal(m1,m2,eps),label+" m+=aT*b");

    m1 = m;
    m1 = T(10) * m1a.transpose() * m1b;
    m2 = T(10) * ma.transpose() * mb;
    Assert(Equal(m1,m2,eps),label+" m=10*aT*b");

    m1 = m;
    m1 += T(10) * m1a.transpose() * m1b;
    m2 = m + T(10) * ma.transpose() * mb;
    Assert(Equal(m1,m2,eps),label+" m+=10*aT*b");

    m1 = m;
    m1 = m1a.transpose() * m1b.transpose();
    m2 = ma.transpose() * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m=aT*bT");

    m1 = m;
    m1 += m1a.transpose() * m1b.transpose();
    m2 = m + ma.transpose() * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m+=aT*bT");

    m1 = m;
    m1 = T(10) * m1a.transpose() * m1b.transpose();
    m2 = T(10) * ma.transpose() * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m=10*aT*bT");

    m1 = m;
    m1 += T(10) * m1a.transpose() * m1b.transpose();
    m2 = m + T(10) * ma.transpose() * mb.transpose();
    Assert(Equal(m1,m2,eps),label+" m+=10*aT*bT");
}
Example #14
0
/*****************************************************************************
 * Function: AdjustSelection
 *
 *****************************************************************************/
static	void
AdjustSelection (
    _DtCanvasStruct	*canvas,
    _DtCvSelectData next)
{
    _DtCvSelectData  start = canvas->select_start;
    _DtCvSelectData  end   = canvas->select_end;

    if (!(Equal(next, end)))
      {
	if (next.line_idx != -1 && next.line_idx == canvas->select_end.line_idx
		&&
	    next.char_idx != -1 && next.char_idx == canvas->select_end.char_idx)
	    return;

	if (GreaterThan(next, end))
	  {
	    if (LessThanEq(start, end))
		_DtCvDrawAreaWithFlags (canvas, end, next,
						0, _DtCvSELECTED_FLAG,
						_DtCvBAD_TYPE, NULL);

	    else if (GreaterThanEq(start, next))
		_DtCvDrawAreaWithFlags (canvas, end, next,
						_DtCvSELECTED_FLAG, 0,
						_DtCvBAD_TYPE, NULL);

	    else /* end < start < next */
	      {
		_DtCvDrawAreaWithFlags (canvas, end  , start,
						_DtCvSELECTED_FLAG, 0,
						_DtCvBAD_TYPE, NULL);
		_DtCvDrawAreaWithFlags (canvas, start, next ,
						0, _DtCvSELECTED_FLAG,
						_DtCvBAD_TYPE, NULL);
	      }
	  }
	else /* if (next < end) */
	  {
	    if (LessThanEq(start, next))
		_DtCvDrawAreaWithFlags (canvas, next, end,
						_DtCvSELECTED_FLAG, 0,
						_DtCvBAD_TYPE, NULL);

	    else if (GreaterThanEq(start, end))
		_DtCvDrawAreaWithFlags (canvas, next, end,
						0, _DtCvSELECTED_FLAG,
						_DtCvBAD_TYPE, NULL);

	    else /* next < start < end */
	      {
		_DtCvDrawAreaWithFlags (canvas, start, end  ,
						_DtCvSELECTED_FLAG, 0,
						_DtCvBAD_TYPE, NULL);
		_DtCvDrawAreaWithFlags (canvas, next , start,
						0, _DtCvSELECTED_FLAG,
						_DtCvBAD_TYPE, NULL);
	      }
	  }
      }

    canvas->select_end = next;
}
Example #15
0
void SwigType_typename_replace(SwigType *t, String *pat, String *rep) {
  String *nt;
  int i, ilen;
  List *elem;

  if (!Strstr(t, pat))
    return;

  if (Equal(t, pat)) {
    Replace(t, pat, rep, DOH_REPLACE_ANY);
    return;
  }
  nt = NewStringEmpty();
  elem = SwigType_split(t);
  ilen = Len(elem);
  for (i = 0; i < ilen; i++) {
    String *e = Getitem(elem, i);
    if (SwigType_issimple(e)) {
      if (Equal(e, pat)) {
	/* Replaces a type of the form 'pat' with 'rep<args>' */
	Replace(e, pat, rep, DOH_REPLACE_ANY);
      } else if (SwigType_istemplate(e)) {
	/* Replaces a type of the form 'pat<args>' with 'rep' */
	if (Equal(e, pat)) {
	  String *repbase = SwigType_templateprefix(rep);
	  Replace(e, pat, repbase, DOH_REPLACE_ID | DOH_REPLACE_FIRST);
	  Delete(repbase);
	}
	{
	  String *tsuffix;
	  List *tparms = SwigType_parmlist(e);
	  int j, jlen;
	  String *nt = SwigType_templateprefix(e);
	  Append(nt, "<(");
	  jlen = Len(tparms);
	  for (j = 0; j < jlen; j++) {
	    SwigType_typename_replace(Getitem(tparms, j), pat, rep);
	    Append(nt, Getitem(tparms, j));
	    if (j < (jlen - 1))
	      Putc(',', nt);
	  }
	  tsuffix = SwigType_templatesuffix(e);
	  Printf(nt, ")>%s", tsuffix);
	  Delete(tsuffix);
	  Clear(e);
	  Append(e, nt);
	  Delete(nt);
	  Delete(tparms);
	}
      } else if (Swig_scopename_check(e)) {
	String *first, *rest;
	first = Swig_scopename_first(e);
	rest = Swig_scopename_suffix(e);
	SwigType_typename_replace(rest, pat, rep);
	SwigType_typename_replace(first, pat, rep);
	Clear(e);
	Printv(e, first, "::", rest, NIL);
	Delete(first);
	Delete(rest);
      }
    } else if (SwigType_isfunction(e)) {
      int j, jlen;
      List *fparms = SwigType_parmlist(e);
      Clear(e);
      Append(e, "f(");
      jlen = Len(fparms);
      for (j = 0; j < jlen; j++) {
	SwigType_typename_replace(Getitem(fparms, j), pat, rep);
	Append(e, Getitem(fparms, j));
	if (j < (jlen - 1))
	  Putc(',', e);
      }
      Append(e, ").");
      Delete(fparms);
    } else if (SwigType_isarray(e)) {
      Replace(e, pat, rep, DOH_REPLACE_ID);
    }
    Append(nt, e);
  }
  Clear(t);
  Append(t, nt);
  Delete(nt);
  Delete(elem);
}
Example #16
0
 priority_queue(DistanceMap const& distance,
                IndexInHeapPropertyMap const& indexInHeap = IndexInHeapPropertyMap(),
                Better const& better = Better(), Size containerReserve = 100, Equal const& equal = Equal())
     : Base(distance, indexInHeap, better, containerReserve, equal) {}
Example #17
0
void Swig_fragment_emit(Node *n) {
  String *code;
  char *pc, *tok;
  String *t;
  String *mangle = 0;
  String *name = 0;
  String *type = 0;

  if (!fragments) {
    Swig_warning(WARN_FRAGMENT_NOT_FOUND, Getfile(n), Getline(n), "Fragment '%s' not found.\n", name);
    return;
  }


  name = Getattr(n, "value");
  if (!name) {
    name = n;
  }
  type = Getattr(n, "type");
  if (type) {
    mangle = Swig_string_mangle(type);
  }

  if (debug)
    Printf(stdout, "looking fragment %s %s\n", name, type);
  t = Copy(name);
  tok = Char(t);
  pc = char_index(tok, ',');
  if (pc)
    *pc = 0;
  while (tok) {
    String *name = NewString(tok);
    if (mangle)
      Append(name, mangle);
    if (looking_fragments && Getattr(looking_fragments, name)) {
      return;
    }
    code = Getattr(fragments, name);
    if (debug)
      Printf(stdout, "looking subfragment %s\n", name);
    if (code && (Strcmp(code, "ignore") != 0)) {
      String *section = Getmeta(code, "section");
      Hash *nn = Getmeta(code, "kwargs");
      if (!looking_fragments)
	looking_fragments = NewHash();
      Setattr(looking_fragments, name, "1");
      while (nn) {
	if (Equal(Getattr(nn, "name"), "fragment")) {
	  if (debug)
	    Printf(stdout, "emitting fragment %s %s\n", nn, type);
	  Setfile(nn, Getfile(n));
	  Setline(nn, Getline(n));
	  Swig_fragment_emit(nn);
	}
	nn = nextSibling(nn);
      }
      if (section) {
	File *f = Swig_filebyname(section);
	if (!f) {
	  Swig_error(Getfile(code), Getline(code), "Bad section '%s' in %%fragment declaration for code fragment '%s'\n", section, name);
	} else {
	  if (debug)
	    Printf(stdout, "emitting subfragment %s %s\n", name, section);
	  if (debug)
	    Printf(f, "/* begin fragment %s */\n", name);
	  Printf(f, "%s\n", code);
	  if (debug)
	    Printf(f, "/* end fragment %s */\n\n", name);
	  Setattr(fragments, name, "ignore");
	  Delattr(looking_fragments, name);
	}
      }
    } else if (!code && type) {
      SwigType *rtype = SwigType_typedef_resolve_all(type);
      if (!Equal(type, rtype)) {
	String *name = Copy(Getattr(n, "value"));
	String *mangle = Swig_string_mangle(type);
	Append(name, mangle);
	Setfile(name, Getfile(n));
	Setline(name, Getline(n));
	Swig_fragment_emit(name);
	Delete(mangle);
	Delete(name);
      }
      Delete(rtype);
    }

    if (!code) {
      Swig_warning(WARN_FRAGMENT_NOT_FOUND, Getfile(n), Getline(n), "Fragment '%s' not found.\n", name);
    }
    tok = pc ? pc + 1 : 0;
    if (tok) {
      pc = char_index(tok, ',');
      if (pc)
	*pc = 0;
    }
    Delete(name);
  }
  Delete(t);
}
Example #18
0
 /// see d_ary_heap_indirect for public methods (push(val), pop(), Val const& top()), adjust_top()
 priority_queue(Size containerReserve = 1 + Arity + Arity * Arity)
     : Base(DistanceMap(), IndexInHeapPropertyMap(), Better(), containerReserve, Equal()) {}
Example #19
0
void Scanner_locator(Scanner *s, String *loc) {
  static Locator *locs = 0;
  static int expanding_macro = 0;

  if (!follow_locators) {
    if (Equal(loc, "/*@SWIG@*/")) {
      /* End locator. */
      if (expanding_macro)
	--expanding_macro;
    } else {
      /* Begin locator. */
      ++expanding_macro;
    }
    /* Freeze line number processing in Scanner */
    freeze_line(s,expanding_macro);
  } else {
    int c;
    Locator *l;
    (void)Seek(loc, 7, SEEK_SET);
    c = Getc(loc);
    if (c == '@') {
      /* Empty locator.  We pop the last location off */
      if (locs) {
	Scanner_set_location(s, locs->filename, locs->line_number);
	cparse_file = locs->filename;
	cparse_line = locs->line_number;
	l = locs->next;
	free(locs);
	locs = l;
      }
      return;
    }

    /* We're going to push a new location */
    l = (Locator *) malloc(sizeof(Locator));
    l->filename = cparse_file;
    l->line_number = cparse_line;
    l->next = locs;
    locs = l;

    /* Now, parse the new location out of the locator string */
    {
      String *fn = NewStringEmpty();
      /*      Putc(c, fn); */
      
      while ((c = Getc(loc)) != EOF) {
	if ((c == '@') || (c == ','))
	  break;
	Putc(c, fn);
      }
      cparse_file = Swig_copy_string(Char(fn));
      Clear(fn);
      cparse_line = 1;
      /* Get the line number */
      while ((c = Getc(loc)) != EOF) {
	if ((c == '@') || (c == ','))
	  break;
	Putc(c, fn);
      }
      cparse_line = atoi(Char(fn));
      Clear(fn);
      
      /* Get the rest of it */
      while ((c = Getc(loc)) != EOF) {
	if (c == '@')
	  break;
	Putc(c, fn);
      }
      /*  Swig_diagnostic(cparse_file, cparse_line, "Scanner_set_location\n"); */
      Scanner_set_location(s, cparse_file, cparse_line);
      Delete(fn);
    }
  }
}
Example #20
0
 bool NumericVector2<T>::operator==(const NumericVector2<T>& rhs) const
 {
   return Equal(x, rhs.x) && Equal(y, rhs.y);
 }
Example #21
0
DVector	CPlayerCamera::FindOptimalCameraPosition()
{
	DVector pos;
	VEC_INIT(pos);

	if (!m_pClientDE || !m_hTarget) return pos;

	DVector		up, right, forward, dir;
	DFLOAT		distToOptimal;
	DVector		TargetPlusOffset;
	
	DVector vTargetPos;
	m_pClientDE->GetObjectPos(m_hTarget, &vTargetPos);

	DRotation rRot;
	m_pClientDE->GetObjectRotation(m_hTarget, &rRot);

	if (Equal(vTargetPos, m_vLastTargetPos) && Equal(rRot, m_rLastTargetRot) && m_eCameraMode != DEATH)
	{
		return m_vLastOptPos;
	}
	else
	{
		VEC_COPY(m_vLastTargetPos, vTargetPos);
		ROT_COPY(m_rLastTargetRot, rRot);
	}
		
	DVector vTemp;

	if (m_eCameraMode == DEATH)
	{
		VEC_COPY(vTemp, m_TargetDeathOffset);
	}
	else
	{
		VEC_COPY(vTemp, m_TargetChaseOffset);
	}
	VEC_ADD(vTemp, vTargetPos, vTemp);
	VEC_COPY(TargetPlusOffset, vTemp);

	m_pClientDE->GetRotationVectors(&rRot, &up, &right, &forward);

	//	pos = TargetPlusOffset + right*m_OptX + up*m_OptY + forward*m_OptZ;
	
	DVector vTemp1, vTemp2;
	if (m_eCameraMode == DEATH)
	{
		VEC_MULSCALAR(vTemp, right, m_DeathOptX);
		VEC_MULSCALAR(vTemp2, forward, m_DeathOptZ);
	}
	else
	{
		VEC_MULSCALAR(vTemp, right, m_OptX);
		VEC_MULSCALAR(vTemp2, forward, m_OptZ);
	}
	VEC_MULSCALAR(vTemp1, up, m_OptY);

	ClientIntersectQuery iQuery;
	ClientIntersectInfo  iInfo;

	VEC_ADD(vTemp, vTemp, vTemp1);
	VEC_ADD(vTemp, vTemp, vTemp2);
	VEC_ADD(pos, TargetPlusOffset, vTemp);

	VEC_SUB(vTemp, TargetPlusOffset, pos);
	distToOptimal = VEC_MAG(vTemp);

	VEC_SUB(dir, pos, TargetPlusOffset);
	VEC_NORM(dir);

	VEC_COPY(iQuery.m_From, TargetPlusOffset);
	VEC_COPY(iQuery.m_To, pos);

	if (m_pClientDE->IntersectSegment(&iQuery, &iInfo))
	{		
		VEC_SUB(vTemp, iInfo.m_Point, TargetPlusOffset);

		// If there was something in the way, move in front of that thing.
		if (VEC_MAG(vTemp) < distToOptimal)
		{
			VEC_ADD(pos, iInfo.m_Point, iInfo.m_Plane.m_Normal);
		}
	}

#ifdef DOING_EXTRA_CHECKS

	// Make sure we aren't clipping into walls...
	
	DFLOAT fClipDistance	= 100.0f; // 15.0f;
	DBOOL bClipRightIssues	= DTRUE;
	DBOOL bClipUpIssues		= DTRUE;


	// Check for walls to the right...

	VEC_MULSCALAR(vTemp, right, fClipDistance);
	VEC_ADD(vTemp, pos, vTemp);

	VEC_COPY(iQuery.m_From, pos);
	VEC_COPY(iQuery.m_To, vTemp);

	if (m_pClientDE->IntersectSegment(&iQuery, &iInfo))
	{
		VEC_SUB(vTemp, iInfo.m_Point, pos);
		DFLOAT fDist = (fClipDistance - VEC_MAG(vTemp));

		VEC_MULSCALAR(vTemp, right, -fDist)
		VEC_ADD(pos, pos, vTemp);
	}
	else
	{
		bClipRightIssues = DFALSE;
	}


	// If we didn't adjust for a wall to the right, check walls to the left...

	if (!bClipRightIssues)
	{
		VEC_MULSCALAR(vTemp, right, -fClipDistance);
		VEC_ADD(vTemp, pos, vTemp);

		VEC_COPY(iQuery.m_From, pos);
		VEC_COPY(iQuery.m_To, vTemp);

		if (m_pClientDE->IntersectSegment(&iQuery, &iInfo))
		{
			VEC_SUB(vTemp, iInfo.m_Point, pos);
			DFLOAT fDist = (fClipDistance - VEC_MAG(vTemp));

			VEC_MULSCALAR(vTemp, right, fDist)
			VEC_ADD(pos, pos, vTemp);
		}
	}
			

	// Check for ceilings...

	VEC_MULSCALAR(vTemp, up, fClipDistance);
	VEC_ADD(vTemp, pos, vTemp);

	VEC_COPY(iQuery.m_From, pos);
	VEC_COPY(iQuery.m_To, vTemp);

	if (m_pClientDE->IntersectSegment(&iQuery, &iInfo))
	{
		VEC_SUB(vTemp, iInfo.m_Point, pos);
		DFLOAT fDist = (fClipDistance - VEC_MAG(vTemp));

		VEC_MULSCALAR(vTemp, up, -fDist)
		VEC_ADD(pos, pos, vTemp);
	}
	else
	{
		bClipUpIssues = DFALSE;
	}


	// If we didn't hit any ceilings, check for floors...

	if (!bClipUpIssues)
	{
		VEC_MULSCALAR(vTemp, up, -fClipDistance);
		VEC_ADD(vTemp, pos, vTemp);

		VEC_COPY(iQuery.m_From, pos);
		VEC_COPY(iQuery.m_To, vTemp);

		if (m_pClientDE->IntersectSegment(&iQuery, &iInfo))
		{
			VEC_SUB(vTemp, iInfo.m_Point, pos);
			DFLOAT fDist = (fClipDistance - VEC_MAG(vTemp));

			VEC_MULSCALAR(vTemp, up, fDist)
			VEC_ADD(pos, pos, vTemp);
		}
	}
#endif  // DOING_EXTRA_CHECKS 

	VEC_COPY(m_vLastOptPos, pos);
	return pos;
}
Example #22
0
 bool NumericVector2<T>::IsZero(void) const
 {
   return Equal(x, T(0)) && Equal(y, T(0));
 }
Example #23
0
bool test(accelerator_view &rv)
{
    int data[] = {0, 0, 0, 0};
    vector<int> Flags(data, data + sizeof(data) / sizeof(int));
    extent<1> eflags(sizeof(data) / sizeof(int));
    array<int, 1> aFlag(eflags, Flags.begin(), rv);

    const int size = 100;

    vector<int> A(size);

    for(int i = 0; i < size; i++)
    {
        A[i] = INIT_VALUE;
    }

    extent<1> e(size);

    array<int, 1> aA(e, A.begin(), rv);

    parallel_for_each(aA.extent, [&](index<1>idx) __GPU
    {
        type arr[10];

        arr[0] = 0; arr[1] = 1; arr[2] = 2; arr[3] = 3; arr[4] = 4; 
        arr[5] = 5; arr[6] = 6; arr[7] = 7; arr[8] = 8; arr[9] = 9;

        type *p = NULL;

        if (aFlag[0] == 0)
            p = &arr[0];
        else
            p = &arr[1];

        p++;

        type *p2 = p;

        if (!Equal(*p2, (type)1))
            aA[idx] = 1;

        p2--;

        type *p3 = p2;

        if (!Equal(*p3, (type)0))
            aA[idx] = 1;

        p = &arr[0];

        if (aFlag[0] == 0)
            p2 = &arr[9];
        else
            p2 = &arr[8];

        int diff = p2 - p;

        if ((diff != 9) || (diff != (&arr[9] - &arr[0])))
            aA[idx] = 1;

        p2 = p + 9;

        if (!Equal(*p2, (type)9))
            aA[idx] = 1;
    });
Example #24
0
 bool NumericVector2<T>::IsParallel(const NumericVector2<T>& rhs) const
 {
   return Equal(Cross(rhs), T(0));
 }
Example #25
0
int main()
{
    int total,i,j,k,l;
    FILE *fin  = fopen ("transform.in", "r");
    FILE *fout = fopen ("transform.out", "w");
    fscanf(fin,"%d",&n);
    for(i=1; i<=n; i++)
    {
        fscanf(fin,"%s",&ori[i][1]);
    }
    for(i=1; i<=n; i++)
    {
        fscanf(fin,"%s",&tar[i][1]);
    }
    for(i=1,k=n; i<=n; i++,k--)
    {
        for(j=1; j<=n; j++)
            map1[j][k]=ori[i][j];
    }
    if(Equal(map1,tar))
    {
        fprintf(fout,"1\n");
        return 0;
    }
    for(i=1,k=n; i<=n; i++,k--)
    {
        for(j=1,l=n; j<=n; j++,l--)
            map2[k][l]=ori[i][j];
    }
    if(Equal(map2,tar))
    {
        fprintf(fout,"2\n");
        return 0;
    }
    for(i=1; i<=n; i++)
    {
        for(j=1,k=n; j<=n; j++,k--)
            map3[k][i]=ori[i][j];
    }
    if(Equal(map3,tar))
    {
        fprintf(fout,"3\n");
        return 0;
    }
    for(i=1; i<=n; i++)
    {
        for(j=1,k=n; j<=n; j++,k--)
            map4[i][k]=ori[i][j];
    }
    if(Equal(map4,tar))
    {
        fprintf(fout,"4\n");
        return 0;
    }
    for(i=1,k=n; i<=n; i++,k--)
    {
        for(j=1; j<=n; j++)
            map5[j][k]=map4[i][j];
    }
    if(Equal(map5,tar))
    {
        fprintf(fout,"5\n");
        return 0;
    }
    for(i=1,k=n; i<=n; i++,k--)
    {
        for(j=1,l=n; j<=n; j++,l--)
            map6[k][l]=map4[i][j];
    }
    if(Equal(map6,tar))
    {
        fprintf(fout,"5\n");
        return 0;
    }
    for(i=1; i<=n; i++)
    {
        for(j=1,k=n; j<=n; j++,k--)
            map7[k][i]=map4[i][j];
    }
    if(Equal(map7,tar))
    {
        fprintf(fout,"5\n");
        return 0;
    }
    for(i=1; i<=n; i++)
    {
        for(j=1; j<=n; j++)
            map8[i][j]=ori[i][j];
    }
    if(Equal(map8,tar))
    {
        fprintf(fout,"6\n");
        return 0;
    }
    fprintf(fout,"7\n");
    return 0;
}
Example #26
0
 bool NumericVector2<T>::IsOrthogonal(const NumericVector2<T>& rhs) const
 {
   return Equal(Dot(rhs), T(0));
 }
Example #27
0
 /**
  * Default constructor.
  */
 MyHashMap(Hash const& hash = Hash(), Equal const& equal = Equal())
         : Table(MyHashMapHashWrapper<K,V,Hash,Equal>(hash, equal),
                 MyHashMapHashWrapper<K,V,Hash,Equal>(hash, equal)) {
 }
	void OneTaskInverseDynamicsJL::update(const ros::Time& time, const ros::Duration& period)
	{
		// get joint positions
  		for(int i=0; i < joint_handles_.size(); i++) 
  		{
    		joint_msr_states_.q(i) = joint_handles_[i].getPosition();
    		joint_msr_states_.qdot(i) = joint_handles_[i].getVelocity();
    	}

    	// clearing msgs before publishing
    	msg_err_.data.clear();
    	msg_pose_.data.clear();
    	
    	if (cmd_flag_)
    	{
    		// resetting N and tau(t=0) for the highest priority task
    		N_trans_ = I_;	
    		SetToZero(tau_);

    		// computing Inertia, Coriolis and Gravity matrices
		    id_solver_->JntToMass(joint_msr_states_.q, M_);
		    id_solver_->JntToCoriolis(joint_msr_states_.q, joint_msr_states_.qdot, C_);
		    id_solver_->JntToGravity(joint_msr_states_.q, G_);
		    G_.data.setZero();

		    // computing the inverse of M_ now, since it will be used often
		    pseudo_inverse(M_.data,M_inv_,false); //M_inv_ = M_.data.inverse(); 


	    	// computing Jacobian J(q)
	    	jnt_to_jac_solver_->JntToJac(joint_msr_states_.q,J_);

	    	// computing the distance from the mid points of the joint ranges as objective function to be minimized
	    	phi_ = task_objective_function(joint_msr_states_.q);

	    	// using the first step to compute jacobian of the tasks
	    	if (first_step_)
	    	{
	    		J_last_ = J_;
	    		phi_last_ = phi_;
	    		first_step_ = 0;
	    		return;
	    	}

	    	// computing the derivative of Jacobian J_dot(q) through numerical differentiation
	    	J_dot_.data = (J_.data - J_last_.data)/period.toSec();

	    	// computing forward kinematics
	    	fk_pos_solver_->JntToCart(joint_msr_states_.q,x_);

	    	if (Equal(x_,x_des_,0.05))
	    	{
	    		ROS_INFO("On target");
	    		cmd_flag_ = 0;
	    		return;	    		
	    	}

	    	// pushing x to the pose msg
	    	for (int i = 0; i < 3; i++)
	    		msg_pose_.data.push_back(x_.p(i));

	    	// setting marker parameters
	    	set_marker(x_,msg_id_);

	    	// computing end-effector position/orientation error w.r.t. desired frame
	    	x_err_ = diff(x_,x_des_);

	    	x_dot_ = J_.data*joint_msr_states_.qdot.data;    	

	    	// setting error reference
	    	for(int i = 0; i < e_ref_.size(); i++)
		    {
	    		// e = x_des_dotdot + Kd*(x_des_dot - x_dot) + Kp*(x_des - x)
	    		e_ref_(i) =  -Kd_(i)*(x_dot_(i)) + Kp_(i)*x_err_(i);
    			msg_err_.data.push_back(e_ref_(i));
	    	}

    		// computing b = J*M^-1*(c+g) - J_dot*q_dot
    		b_ = J_.data*M_inv_*(C_.data + G_.data) - J_dot_.data*joint_msr_states_.qdot.data;

	    	// computing omega = J*M^-1*N^T*J
	    	omega_ = J_.data*M_inv_*N_trans_*J_.data.transpose();

	    	// computing lambda = omega^-1
	    	pseudo_inverse(omega_,lambda_);
	    	//lambda_ = omega_.inverse();

	    	// computing nullspace
	    	N_trans_ = N_trans_ - J_.data.transpose()*lambda_*J_.data*M_inv_;  	    		

	    	// finally, computing the torque tau
	    	tau_.data = J_.data.transpose()*lambda_*(e_ref_ + b_) + N_trans_*(Eigen::Matrix<double,7,1>::Identity(7,1)*(phi_ - phi_last_)/(period.toSec()));

	    	// saving J_ and phi of the last iteration
	    	J_last_ = J_;
	    	phi_last_ = phi_;
	
    	}

    	// set controls for joints
    	for (int i = 0; i < joint_handles_.size(); i++)
    	{
    		if(cmd_flag_)
    			joint_handles_[i].setCommand(tau_(i));
    		else
       			joint_handles_[i].setCommand(PIDs_[i].computeCommand(joint_des_states_.q(i) - joint_msr_states_.q(i),period));
    	}

    	// publishing markers for visualization in rviz
    	pub_marker_.publish(msg_marker_);
    	msg_id_++;

	    // publishing error 
	    pub_error_.publish(msg_err_);
	    // publishing pose 
	    pub_pose_.publish(msg_pose_);
	    ros::spinOnce();

	}
Example #29
0
File: templ.c Project: kkaempf/swig
static Node *template_locate(String *name, Parm *tparms, Symtab *tscope) {
  Node *n = 0;
  String *tname = 0;
  Node *templ;
  Symtab *primary_scope = 0;
  List *possiblepartials = 0;
  Parm *p;
  Parm *parms = 0;
  Parm *targs;
  ParmList *expandedparms;
  int *priorities_matrix = 0;
  int max_possible_partials = 0;
  int posslen = 0;

  /* Search for primary (unspecialized) template */
  templ = Swig_symbol_clookup(name, 0);

  if (template_debug) {
    tname = Copy(name);
    SwigType_add_template(tname, tparms);
    Printf(stdout, "\n");
    Swig_diagnostic(cparse_file, cparse_line, "template_debug: Searching for match to: '%s'\n", tname);
    Delete(tname);
    tname = 0;
  }

  if (templ) {
    tname = Copy(name);
    parms = CopyParmList(tparms);

    /* All template specializations must be in the primary template's scope, store the symbol table for this scope for specialization lookups */
    primary_scope = Getattr(templ, "sym:symtab");

    /* Add default values from primary template */
    targs = Getattr(templ, "templateparms");
    expandedparms = Swig_symbol_template_defargs(parms, targs, tscope, primary_scope);

    /* reduce the typedef */
    p = expandedparms;
    while (p) {
      SwigType *ty = Getattr(p, "type");
      if (ty) {
	SwigType *nt = Swig_symbol_type_qualify(ty, tscope);
	Setattr(p, "type", nt);
	Delete(nt);
      }
      p = nextSibling(p);
    }
    SwigType_add_template(tname, expandedparms);

    /* Search for an explicit (exact) specialization. Example: template<> class name<int> { ... } */
    {
      if (template_debug) {
	Printf(stdout, "    searching for : '%s' (explicit specialization)\n", tname);
      }
      n = Swig_symbol_clookup_local(tname, primary_scope);
      if (!n) {
	SwigType *rname = Swig_symbol_typedef_reduce(tname, tscope);
	if (!Equal(rname, tname)) {
	  if (template_debug) {
	    Printf(stdout, "    searching for : '%s' (explicit specialization with typedef reduction)\n", rname);
	  }
	  n = Swig_symbol_clookup_local(rname, primary_scope);
	}
	Delete(rname);
      }
      if (n) {
	Node *tn;
	String *nodeType = nodeType(n);
	if (Equal(nodeType, "template")) {
	  if (template_debug) {
	    Printf(stdout, "    explicit specialization found: '%s'\n", Getattr(n, "name"));
	  }
	  goto success;
	}
	tn = Getattr(n, "template");
	if (tn) {
	  if (template_debug) {
	    Printf(stdout, "    previous instantiation found: '%s'\n", Getattr(n, "name"));
	  }
	  n = tn;
	  goto success;	  /* Previously wrapped by a template instantiation */
	}
	Swig_error(cparse_file, cparse_line, "'%s' is not defined as a template. (%s)\n", name, nodeType(n));
	Delete(tname);
	Delete(parms);
	return 0;	  /* Found a match, but it's not a template of any kind. */
      }
    }

    /* Search for partial specializations.
     * Example: template<typename T> class name<T *> { ... } 

     * There are 3 types of template arguments:
     * (1) Template type arguments
     * (2) Template non type arguments
     * (3) Template template arguments
     * only (1) is really supported for partial specializations
     */

    /* Rank each template parameter against the desired template parameters then build a matrix of best matches */
    possiblepartials = NewList();
    {
      char tmp[32];
      List *partials;

      partials = Getattr(templ, "partials"); /* note that these partial specializations do not include explicit specializations */
      if (partials) {
	Iterator pi;
	int parms_len = ParmList_len(parms);
	int *priorities_row;
	max_possible_partials = Len(partials);
	priorities_matrix = (int *)malloc(sizeof(int) * max_possible_partials * parms_len); /* slightly wasteful allocation for max possible matches */
	priorities_row = priorities_matrix;
	for (pi = First(partials); pi.item; pi = Next(pi)) {
	  Parm *p = parms;
	  int all_parameters_match = 1;
	  int i = 1;
	  Parm *partialparms = Getattr(pi.item, "partialparms");
	  Parm *pp = partialparms;
	  String *templcsymname = Getattr(pi.item, "templcsymname");
	  if (template_debug) {
	    Printf(stdout, "    checking match: '%s' (partial specialization)\n", templcsymname);
	  }
	  if (ParmList_len(partialparms) == parms_len) {
	    while (p && pp) {
	      SwigType *t;
	      sprintf(tmp, "$%d", i);
	      t = Getattr(p, "type");
	      if (!t)
		t = Getattr(p, "value");
	      if (t) {
		EMatch match = does_parm_match(t, Getattr(pp, "type"), tmp, tscope, priorities_row + i - 1);
		if (match < (int)PartiallySpecializedMatch) {
		  all_parameters_match = 0;
		  break;
		}
	      }
	      i++;
	      p = nextSibling(p);
	      pp = nextSibling(pp);
	    }
	    if (all_parameters_match) {
	      Append(possiblepartials, pi.item);
	      priorities_row += parms_len;
	    }
	  }
	}
      }
    }

    posslen = Len(possiblepartials);
    if (template_debug) {
      int i;
      if (posslen == 0)
	Printf(stdout, "    matched partials: NONE\n");
      else if (posslen == 1)
	Printf(stdout, "    chosen partial: '%s'\n", Getattr(Getitem(possiblepartials, 0), "templcsymname"));
      else {
	Printf(stdout, "    possibly matched partials:\n");
	for (i = 0; i < posslen; i++) {
	  Printf(stdout, "      '%s'\n", Getattr(Getitem(possiblepartials, i), "templcsymname"));
	}
      }
    }

    if (posslen > 1) {
      /* Now go through all the possibly matched partial specialization templates and look for a non-ambiguous match.
       * Exact matches rank the highest and deduced parameters are ranked by how specialized they are, eg looking for
       * a match to const int *, the following rank (highest to lowest):
       *   const int * (exact match)
       *   const T *
       *   T *
       *   T
       *
       *   An ambiguous example when attempting to match as either specialization could match: %template() X<int *, double *>;
       *   template<typename T1, typename T2> X class {};  // primary template
       *   template<typename T1> X<T1, double *> class {}; // specialization (1)
       *   template<typename T2> X<int *, T2> class {};    // specialization (2)
       */
      if (template_debug) {
	int row, col;
	int parms_len = ParmList_len(parms);
	Printf(stdout, "      parameter priorities matrix (%d parms):\n", parms_len);
	for (row = 0; row < posslen; row++) {
	  int *priorities_row = priorities_matrix + row*parms_len;
	  Printf(stdout, "        ");
	  for (col = 0; col < parms_len; col++) {
	    Printf(stdout, "%5d ", priorities_row[col]);
	  }
	  Printf(stdout, "\n");
	}
      }
      {
	int row, col;
	int parms_len = ParmList_len(parms);
	/* Printf(stdout, "      parameter priorities inverse matrix (%d parms):\n", parms_len); */
	for (col = 0; col < parms_len; col++) {
	  int *priorities_col = priorities_matrix + col;
	  int maxpriority = -1;
	  /* 
	     Printf(stdout, "max_possible_partials: %d col:%d\n", max_possible_partials, col);
	     Printf(stdout, "        ");
	     */
	  /* determine the highest rank for this nth parameter */
	  for (row = 0; row < posslen; row++) {
	    int *element_ptr = priorities_col + row*parms_len;
	    int priority = *element_ptr;
	    if (priority > maxpriority)
	      maxpriority = priority;
	    /* Printf(stdout, "%5d ", priority); */
	  }
	  /* Printf(stdout, "\n"); */
	  /* flag all the parameters which equal the highest rank */
	  for (row = 0; row < posslen; row++) {
	    int *element_ptr = priorities_col + row*parms_len;
	    int priority = *element_ptr;
	    *element_ptr = (priority >= maxpriority) ? 1 : 0;
	  }
	}
      }
      {
	int row, col;
	int parms_len = ParmList_len(parms);
	Iterator pi = First(possiblepartials);
	Node *chosenpartials = NewList();
	if (template_debug)
	  Printf(stdout, "      priority flags matrix:\n");
	for (row = 0; row < posslen; row++) {
	  int *priorities_row = priorities_matrix + row*parms_len;
	  int highest_count = 0; /* count of highest priority parameters */
	  for (col = 0; col < parms_len; col++) {
	    highest_count += priorities_row[col];
	  }
	  if (template_debug) {
	    Printf(stdout, "        ");
	    for (col = 0; col < parms_len; col++) {
	      Printf(stdout, "%5d ", priorities_row[col]);
	    }
	    Printf(stdout, "\n");
	  }
	  if (highest_count == parms_len) {
	    Append(chosenpartials, pi.item);
	  }
	  pi = Next(pi);
	}
	if (Len(chosenpartials) > 0) {
	  /* one or more best match found */
	  Delete(possiblepartials);
	  possiblepartials = chosenpartials;
	  posslen = Len(possiblepartials);
	} else {
	  /* no best match found */
	  Delete(chosenpartials);
	}
      }
    }

    if (posslen > 0) {
      String *s = Getattr(Getitem(possiblepartials, 0), "templcsymname");
      n = Swig_symbol_clookup_local(s, primary_scope);
      if (posslen > 1) {
	int i;
	if (n) {
	  Swig_warning(WARN_PARSE_TEMPLATE_AMBIG, cparse_file, cparse_line, "Instantiation of template '%s' is ambiguous,\n", SwigType_namestr(tname));
	  Swig_warning(WARN_PARSE_TEMPLATE_AMBIG, Getfile(n), Getline(n), "  instantiation '%s' used,\n", SwigType_namestr(Getattr(n, "name")));
	}
	for (i = 1; i < posslen; i++) {
	  String *templcsymname = Getattr(Getitem(possiblepartials, i), "templcsymname");
	  Node *ignored_node = Swig_symbol_clookup_local(templcsymname, primary_scope);
	  assert(ignored_node);
	  Swig_warning(WARN_PARSE_TEMPLATE_AMBIG, Getfile(ignored_node), Getline(ignored_node), "  instantiation '%s' ignored.\n", SwigType_namestr(Getattr(ignored_node, "name")));
	}
      }
    }

    if (!n) {
      if (template_debug) {
	Printf(stdout, "    chosen primary template: '%s'\n", Getattr(templ, "name"));
      }
      n = templ;
    }
  } else {
    if (template_debug) {
      Printf(stdout, "    primary template not found\n");
    }
    /* Give up if primary (unspecialized) template not found as specializations will only exist if there is a primary template */
    n = 0;
  }

  if (!n) {
    Swig_error(cparse_file, cparse_line, "Template '%s' undefined.\n", name);
  } else if (n) {
    String *nodeType = nodeType(n);
    if (!Equal(nodeType, "template")) {
      Swig_error(cparse_file, cparse_line, "'%s' is not defined as a template. (%s)\n", name, nodeType);
      n = 0;
    }
  }
success:
  Delete(tname);
  Delete(possiblepartials);
  if ((template_debug) && (n)) {
    /*
    Printf(stdout, "Node: %p\n", n);
    Swig_print_node(n);
    */
    Printf(stdout, "    chosen template:'%s'\n", Getattr(n, "name"));
  }
  Delete(parms);
  free(priorities_matrix);
  return n;
}
Example #30
0
int main() {
	scanf("%lld %lld %lld %lld %lld %lld %lld %lld", &A1, &B1, &C1, &A2, &B2, &C2, &xy[0], &xy[1]);
	long long W, W1, W2, Wxy[2];
	long long lline, X, Y;
	double dline, dxlen, dylen;
	long long P1 = 0, P2 = 0;
	bool bool1, bool2, bool3, bool4;

	W  =  A1*B2 - A2*B1;
	W1 = -C1*B2 + C2*B1;
	W2 = -A1*C2 + A2*C1;

	Wxy[0] = W1 / W;
	Wxy[1] = W2 / W;
	double Dxy[2];
	Dxy[0] = ((double) W1) / ((double) W);
	Dxy[1] = ((double) W2) / ((double) W);
	
	long long qx = Wxy[0];
	long long qy = Wxy[1];
	long long i, j, goodx, goody;
		
	double minimum = -1, len;
	for (i = Wxy[0] - 120; i < Wxy[0] + 120; ++i)
		for (j = Wxy[1] - 120; j < Wxy[1] + 120; ++j) 
			if (TotalCheck(i, j)) {
				len = Distance(i, j, Dxy);
				if (minimum > len || minimum < 0) { minimum = len; goodx = i; goody = j; }
			}
			
	if (minimum > 0) { printf("%lld %lld", goodx, goody); return 0; }
	
// Do dobra, zlosliwie to jest malutki kat miedzy prostymi

	if (A1 == 0 || A2 == 0 || B1 == 0 || B2 == 0) { // a tak zebym potem nie musial uwazac na jakies syfy...
		if (A1 == 0) { 
				dxlen = ((double) -B2) / ((double) A2) + Wxy[0];
				if (dxlen > Wxy[0]) { // +
					X = (long long) (ceil(dxlen) + (Equal(ceil(dxlen), dxlen) ? 1 : 0));
					Y = (A2 * B2 > 0 ? -1 : 1) + ((long long) (((double) -C2) / ((double) B2)));
				} else { // -
					X = (long long) (floor(dxlen) - (Equal(floor(dxlen), dxlen) ? 1 : 0));
					Y = (A2 * B2 > 0 ? 1 : -1) + ((long long) (((double) -C2) / ((double) B2)));
				}
		}
		if (A2 == 0) {
				dxlen = ((double) -B1) / ((double) A1) + Wxy[0];
				if (dxlen > Wxy[0]) { // +
					X = (long long) (ceil(dxlen) + (Equal(ceil(dxlen), dxlen) ? 1 : 0));
					Y = (A1 * B1 > 0 ? -1 : 1) + ((long long) (((double) -C1) / ((double) B1)));
				} else { // -
					X = (long long) (floor(dxlen) - (Equal(floor(dxlen), dxlen) ? 1 : 0));
					Y = (A1 * B1 > 0 ? 1 : -1) + ((long long) (((double) -C1) / ((double) B1)));
				}
		}
		if (B1 == 0) {
				dxlen = ((double) -A2) / ((double) B2) + Wxy[1];
				if (dxlen > Wxy[1]) { // +
					Y = (long long) (ceil(dxlen) + (Equal(ceil(dxlen), dxlen) ? 1 : 0));
					X = (A2 * B2 > 0 ? -1 : 1) + ((long long) (((double) -C2) / ((double) A2)));
				} else { // -
					Y = (long long) (floor(dxlen) - (Equal(floor(dxlen), dxlen) ? 1 : 0));
					X = (A2 * B2 > 0 ? 1 : -1) + ((long long) (((double) -C2) / ((double) A2)));
				}
		}
		if (B2 == 0) {  
				dxlen = ((double) -A1) / ((double) B1) + Wxy[1];
				if (dxlen > Wxy[1]) { // +
					Y = (long long) (ceil(dxlen) + (Equal(ceil(dxlen), dxlen) ? 1 : 0));
					X = (A1 * B1 > 0 ? -1 : 1) + ((long long) (((double) -C1) / ((double) A1)));
				} else { // -
					Y = (long long) (floor(dxlen) - (Equal(floor(dxlen), dxlen) ? 1 : 0));
					X = (A1 * B1 > 0 ? 1 : -1) + ((long long) (((double) -C1) / ((double) A1)));
				}
		}
		printf("%lld %lld", X, Y);
		return 0;
	}

	lline = Wxy[0];

//	long long mod = (W*B1*B2);
//	long long modx = W1 % mod;
//	long long mody = W2 % mod;
//	long long a1x = -(A1 % mod);
//	long long a2x = -(A2 % mod);

	a1 = A1 * W * B1 * B2; a2 = A2 * W * B1 * B2;
	b1 = B1 * W * B1 * B2; b2 = B2 * W * B1 * B2;
	c1 = C1; c2 = C2;
	x = xy[0] * W * B1 * B2; y = xy[1] * W * B1 * B2;
	
	wx = (-C1*B2 + C2*B1) * B1 * B2;
	wy = (-A1*C2 + A2*C1) * B1 * B2;

	

	long long s;
	if (xy[0] * W > W1) s = 1;
	else s = -1;
	
	while (P1 == P2) {
		P1 = -(A1*lline + C1) / B1;
		P2 = -(A2*lline + C2) / B2;
		if (TotalCheck(lline, P1)) {
			printf("%lld %lld", lline, P1); 
			break;
		} else if (TotalCheck(lline, P2)) {
			printf("%lld %lld", lline, P2);
			break;
		} else {
			P1 = P2;
			lline += s;
		}
	}
	
	return 0;
}