コード例 #1
0
ファイル: lib_mm.c プロジェクト: anbangr/bitvisor-dev
void *
alloc (unsigned int size)
{
	int *p, len;
	int total;

	if (!size)
		return NULL;
	spinlock_lock (&lock);
	if (!initialized)
		prepare ();
	TST ("alloc enter");
	if (size < 32)
		size = 32;
	len = (size + sizeof (int) - 1) / sizeof (int);
	p = heap + heaplen - 1;
	while (*p) {
		if (*p < 0) {
			if (-*p >= len)
				goto found;
			p += *p - 1;
		} else {
			p -= *p + 1;
		}
	}
	printf ("allocating %u\n", size);
	total = 0;
	p = heap + heaplen - 1;
	while (*p) {
		if (*p < 0) {
			p += *p - 1;
		} else {
			total += *p + 1;
			p -= *p + 1;
		}
	}
	printf ("allocated %u\n", total * (unsigned int)sizeof (*p));
	printf ("size of heap %u\n", heaplen * (unsigned int)sizeof (*p));
	panic ("out of memory");
found:
	if (-*p <= len + 1) {
		*p = -*p;
		p -= *p;
	} else {
		*p += len + 1;
		p += *p - 1;
		*p = len;
		p -= len;
	}
	TST ("alloc exit");
	spinlock_unlock (&lock);
	return p;
}
コード例 #2
0
/* This function allocates the first available block in the tmp file */
long allocate()
{
	int	i;
	long	offset;

	/* search for the first byte with a free bit set */
	for (i = 0; i < sizeof bitmap && bitmap[i] == 0; i++)
	{
	}

	/* if we hit the end of the bitmap, return the end of the file */
	if (i == sizeof bitmap)
	{
		offset = lseek(tmpfd, 0L, 2);
	}
	else /* compute the offset for the free block */
	{
		for (i <<= 3; TST(i) == 0; i++)
		{
		}
		offset = (long)i * (long)BLKSIZE;

		/* mark the block as "allocated" */
		CLR(i);
	}

	return offset;
}
コード例 #3
0
void JitArm::lwz(UGeckoInstruction inst)
{
	INSTRUCTION_START
	JITDISABLE(LoadStore)

	ARMReg rA = gpr.GetReg();
	ARMReg rB = gpr.GetReg();
	ARMReg RD = gpr.R(inst.RD);
	LDR(rA, R9, PPCSTATE_OFF(Exceptions));
	CMP(rA, EXCEPTION_DSI);
	FixupBranch DoNotLoad = B_CC(CC_EQ);
	{
		if (inst.RA)
		{
			MOVI2R(rB, inst.SIMM_16);
			ARMReg RA = gpr.R(inst.RA);
			ADD(rB, rB, RA);
		}
		else
			MOVI2R(rB, (u32)inst.SIMM_16);

		MOVI2R(rA, (u32)&Memory::Read_U32);	
		PUSH(4, R0, R1, R2, R3);
		MOV(R0, rB);
		BL(rA);
		MOV(rA, R0);
		POP(4, R0, R1, R2, R3);
		MOV(RD, rA);
		gpr.Unlock(rA, rB);
	}
	SetJumpTarget(DoNotLoad);
	if (SConfig::GetInstance().m_LocalCoreStartupParameter.bSkipIdle &&
		(inst.hex & 0xFFFF0000) == 0x800D0000 &&
		(Memory::ReadUnchecked_U32(js.compilerPC + 4) == 0x28000000 ||
		(SConfig::GetInstance().m_LocalCoreStartupParameter.bWii && Memory::ReadUnchecked_U32(js.compilerPC + 4) == 0x2C000000)) &&
		Memory::ReadUnchecked_U32(js.compilerPC + 8) == 0x4182fff8)
	{
		gpr.Flush();
		fpr.Flush();
		
		// if it's still 0, we can wait until the next event
		TST(RD, RD);
		FixupBranch noIdle = B_CC(CC_NEQ);
		rA = gpr.GetReg();	
		
		MOVI2R(rA, (u32)&PowerPC::OnIdle);
		MOVI2R(R0, PowerPC::ppcState.gpr[inst.RA] + (s32)(s16)inst.SIMM_16); 
		BL(rA);

		gpr.Unlock(rA);
		WriteExceptionExit();

		SetJumpTarget(noIdle);

		//js.compilerPC += 8;
		return;
	}
}
コード例 #4
0
ファイル: lib_mm.c プロジェクト: anbangr/bitvisor-dev
void *
realloc (void *virt, unsigned int len)
{
	int *p, alloclen, copylen;
	void *r;

	if (!virt && !len)
		return NULL;
	if (!virt)
		return alloc (len);
	if (!len) {
		free (virt);
		return NULL;
	}
	spinlock_lock (&lock);
	if (!initialized)
		prepare ();
	TST ("realloc enter");
	if (len < 32)
		len = 32;
	p = heap + heaplen - 1;
	while (*p) {
		if (*p < 0)
			p += *p - 1;
		else if (p - *p == virt)
			goto found;
		else
			p -= *p + 1;
	}
	panic ("reallocating not allocated memory %p", virt);
found:
	alloclen = *p * sizeof *p;
	spinlock_unlock (&lock);
	if (alloclen > len)
		copylen = len;
	else
		copylen = alloclen;
	r = alloc (len);
	if (r) {
		memcpy (r, virt, copylen);
		free (virt);
	}
	TST ("realloc exit");
	return r;
}
コード例 #5
0
int ctl_socket_init(void)
{
    int s = server_socket();
    if (s < 0)
        return -1;

    ctl_handler.fd = s;
    ctl_handler.handler = ctl_rcv_handler;

    TST(add_epoll(&ctl_handler) == 0, -1);
    return 0;
}
コード例 #6
0
ファイル: lib_mm.c プロジェクト: anbangr/bitvisor-dev
void
free (void *m)
{
	int *p, *q, len;

	spinlock_lock (&lock);
	if (!initialized)
		prepare ();
	TST ("free enter");
	p = heap + heaplen - 1;
	q = NULL;
	while (*p) {
		if (*p < 0) {
			if (!q)
				q = p;
			p += *p - 1;
		} else if (p - *p == m) {
			goto found;
		} else {
			q = NULL;
			p -= *p + 1;
		}
	}
	panic ("freeing not allocated memory %p", m);
found:
	len = -*p;
	while (p[len - 1] < 0)
		len += p[len - 1] - 1;
	*p = len;
	if (q) {
		len = *q;
		while (q[len - 1] < 0)
			len += q[len - 1] - 1;
		*q = len;
	}
	TST ("free exit");
	spinlock_unlock (&lock);
}
コード例 #7
0
ファイル: tar.c プロジェクト: abrady/atar
int main(int argc, char **argv)
{
    char *src,*srcf = "foo.tar"; int srcn;
    char *f1,*f1t,*f1f = "temp.in.txt"; int f1n, f1nt;
    char *f2,*f2t,*f2f = "temp.out.txt"; int f2n, f2nt;
    FILE *fp;
    Tar *t;
    src = file_alloc(srcf,&srcn);
    t = tar_from_data(src,srcn);

    printf("testing tar\n");
#define TST(COND,MSG)                                   \
    if(!(COND)){                                        \
        printf("error " MSG " cond " #COND "\n");       \
        exit(1);                                        \
    }
        
    f1 = file_alloc(f1f,&f1n);
    f1t = tar_alloc_cur_file(t,&f1nt);
    TST(0==strcmp(f1f,t->hdr.fname),"not the same filename")
    TST(f1n == f1nt,"f1 lengths not equal");
    TST(0==memcmp(f1,f1t,f1n),"f1 memory not the same");
    free(f1);
    free(f1t);

    TST(tar_next(t),"couldn't move to second archive'd file");
    f2 = file_alloc(f2f,&f2n);
    f2t = tar_cur_file(t,&f2nt);
    TST(0==strcmp(f2f,t->hdr.fname),"not the same filename")
    TST(f2n == f2nt,"f2 lengths not equal");
    TST(0==memcmp(f2,f2t,f2n),"f2 memory not the same");
    free(f2);
    
    TST(!tar_next(t),"able to move to third archive, should be EOF");    
    Tar_Destroy(t);
    printf("done\n");
}
コード例 #8
0
int server_socket(void)
{
    struct sockaddr_un sa;
    int s;

    TST(strlen(RSTP_SERVER_SOCK_NAME) < sizeof(sa.sun_path), -1);

    s = socket(PF_UNIX, SOCK_DGRAM, 0);
    if (s < 0) {
        ERROR("Couldn't open unix socket: %m");
        return -1;
    }

    set_socket_address(&sa, RSTP_SERVER_SOCK_NAME);

    if (bind(s, (struct sockaddr *)&sa, sizeof(sa)) != 0) {
        ERROR("Couldn't bind socket: %m");
        close(s);
        return -1;
    }

    return s;
}
コード例 #9
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x2C(void) {		/* 0xED 0x2C : TST  L */
	TST(L);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #10
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x1C(void) {		/* 0xED 0x1C : TST  E */
	TST(E);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #11
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x24(void) {		/* 0xED 0x24 : TST  H */
	TST(H);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #12
0
ファイル: box.cpp プロジェクト: cdaffara/symbiandump-mw1
EXPORT_C int dBoxBox (const dVector3 p1, const dMatrix3 R1,
	     const dVector3 side1, const dVector3 p2,
	     const dMatrix3 R2, const dVector3 side2,
	     dVector3 normal, dReal *depth, int *return_code,
	     int maxc, dContactGeom *contact, int skip)
{
  const dReal fudge_factor = REAL(1.05);
  dVector3 p,pp,normalC;
  const dReal *normalR = 0;
  dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l;
  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
  dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  A[0] = dMUL(side1[0],REAL(0.5));
  A[1] = dMUL(side1[1],REAL(0.5));
  A[2] = dMUL(side1[2],REAL(0.5));
  B[0] = dMUL(side2[0],REAL(0.5));
  B[1] = dMUL(side2[1],REAL(0.5));
  B[2] = dMUL(side2[2],REAL(0.5));

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
  R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
  R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);

  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);

  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

#define TST(expr1,expr2,norm,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  if (s2 > s) { \
    s = s2; \
    normalR = norm; \
    invert_normal = ((expr1) < 0); \
    code = (cc); \
  }

  s = -dInfinity;
  invert_normal = 0;
  code = 0;

  // separating axis = u1,u2,u3
  TST (pp[0],(A[0] + dMUL(B[0],Q11) + dMUL(B[1],Q12) + dMUL(B[2],Q13)),R1+0,1);
  TST (pp[1],(A[1] + dMUL(B[0],Q21) + dMUL(B[1],Q22) + dMUL(B[2],Q23)),R1+1,2);
  TST (pp[2],(A[2] + dMUL(B[0],Q31) + dMUL(B[1],Q32) + dMUL(B[2],Q33)),R1+2,3);

  // separating axis = v1,v2,v3
  TST (dDOT41(R2+0,p),(dMUL(A[0],Q11) + dMUL(A[1],Q21) + dMUL(A[2],Q31) + B[0]),R2+0,4);
  TST (dDOT41(R2+1,p),(dMUL(A[0],Q12) + dMUL(A[1],Q22) + dMUL(A[2],Q32) + B[1]),R2+1,5);
  TST (dDOT41(R2+2,p),(dMUL(A[0],Q13) + dMUL(A[1],Q23) + dMUL(A[2],Q33) + B[2]),R2+2,6);

  // note: cross product axes need to be scaled when s is computed.
  // normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
  s2 = dFabs(expr1) - (expr2); \
  if (s2 > 0) return 0; \
  l = dSqrt (dMUL((n1),(n1)) + dMUL((n2),(n2)) + dMUL((n3),(n3))); \
  if (l > 0) { \
    s2 = dDIV(s2,l); \
    if (dMUL(s2,fudge_factor) > s) { \
      s = s2; \
      normalR = 0; \
      normalC[0] = dDIV((n1),l); normalC[1] = dDIV((n2),l); normalC[2] = dDIV((n3),l); \
      invert_normal = ((expr1) < 0); \
      code = (cc); \
    } \
  }

  // separating axis = u1 x (v1,v2,v3)
  TST((dMUL(pp[2],R21)-dMUL(pp[1],R31)),(dMUL(A[1],Q31)+dMUL(A[2],Q21)+dMUL(B[1],Q13)+dMUL(B[2],Q12)),0,-R31,R21,7);
  TST((dMUL(pp[2],R22)-dMUL(pp[1],R32)),(dMUL(A[1],Q32)+dMUL(A[2],Q22)+dMUL(B[0],Q13)+dMUL(B[2],Q11)),0,-R32,R22,8);
  TST((dMUL(pp[2],R23)-dMUL(pp[1],R33)),(dMUL(A[1],Q33)+dMUL(A[2],Q23)+dMUL(B[0],Q12)+dMUL(B[1],Q11)),0,-R33,R23,9);

  // separating axis = u2 x (v1,v2,v3)
  TST((dMUL(pp[0],R31)-dMUL(pp[2],R11)),(dMUL(A[0],Q31)+dMUL(A[2],Q11)+dMUL(B[1],Q23)+dMUL(B[2],Q22)),R31,0,-R11,10);
  TST((dMUL(pp[0],R32)-dMUL(pp[2],R12)),(dMUL(A[0],Q32)+dMUL(A[2],Q12)+dMUL(B[0],Q23)+dMUL(B[2],Q21)),R32,0,-R12,11);
  TST((dMUL(pp[0],R33)-dMUL(pp[2],R13)),(dMUL(A[0],Q33)+dMUL(A[2],Q13)+dMUL(B[0],Q22)+dMUL(B[1],Q21)),R33,0,-R13,12);

  // separating axis = u3 x (v1,v2,v3)
  TST((dMUL(pp[1],R11)-dMUL(pp[0],R21)),(dMUL(A[0],Q21)+dMUL(A[1],Q11)+dMUL(B[1],Q33)+dMUL(B[2],Q32)),-R21,R11,0,13);
  TST((dMUL(pp[1],R12)-dMUL(pp[0],R22)),(dMUL(A[0],Q22)+dMUL(A[1],Q12)+dMUL(B[0],Q33)+dMUL(B[2],Q31)),-R22,R12,0,14);
  TST((dMUL(pp[1],R13)-dMUL(pp[0],R23)),(dMUL(A[0],Q23)+dMUL(A[1],Q13)+dMUL(B[0],Q32)+dMUL(B[1],Q31)),-R23,R13,0,15);

#undef TST

  if (!code) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal
  // in global coordinates.
  if (normalR) {
    normal[0] = normalR[0];
    normal[1] = normalR[4];
    normal[2] = normalR[8];
  }
  else {
    dMULTIPLY0_331 (normal,R1,normalC);
  }
  if (invert_normal) {
    normal[0] = -normal[0];
    normal[1] = -normal[1];
    normal[2] = -normal[2];
  }
  *depth = -s;

  // compute contact point(s)

  if (code > 6) {
    // an edge from box 1 touches an edge from box 2.
    // find a point pa on the intersecting edge of box 1
    dVector3 pa;
    dReal sign;
    for (i=0; i<3; i++) pa[i] = p1[i];
    for (j=0; j<3; j++) {
      sign = (dDOT14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
      for (i=0; i<3; i++) pa[i] += dMUL(sign,dMUL(A[j],R1[i*4+j]));
    }

    // find a point pb on the intersecting edge of box 2
    dVector3 pb;
    for (i=0; i<3; i++) pb[i] = p2[i];
    for (j=0; j<3; j++) {
      sign = (dDOT14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
      for (i=0; i<3; i++) pb[i] += dMUL(sign,dMUL(B[j],R2[i*4+j]));
    }

    dReal alpha,beta;
    dVector3 ua,ub;
    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];

    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
    for (i=0; i<3; i++) pa[i] += dMUL(ua[i],alpha);
    for (i=0; i<3; i++) pb[i] += dMUL(ub[i],beta);

    for (i=0; i<3; i++) contact[0].pos[i] = dMUL(REAL(0.5),(pa[i]+pb[i]));
    contact[0].depth = *depth;
    *return_code = code;
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face). define face 'a' to be the reference
  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  // the incident face (the closest face of the other box).

  const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  if (code <= 3) {
    Ra = R1;
    Rb = R2;
    pa = p1;
    pb = p2;
    Sa = A;
    Sb = B;
  }
  else {
    Ra = R2;
    Rb = R1;
    pa = p2;
    pb = p1;
    Sa = B;
    Sb = A;
  }

  // nr = normal vector of reference face dotted with axes of incident box.
  // anr = absolute values of nr.
  dVector3 normal2,nr,anr;
  if (code <= 3) {
    normal2[0] = normal[0];
    normal2[1] = normal[1];
    normal2[2] = normal[2];
  }
  else {
    normal2[0] = -normal[0];
    normal2[1] = -normal[1];
    normal2[2] = -normal[2];
  }
  dMULTIPLY1_331 (nr,Rb,normal2);
  anr[0] = dFabs (nr[0]);
  anr[1] = dFabs (nr[1]);
  anr[2] = dFabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal
  // for the indident face. the other axis numbers of the indicent face
  // are stored in a1,a2.
  int lanr,a1,a2;
  if (anr[1] > anr[0]) {
    if (anr[1] > anr[2]) {
      a1 = 0;
      lanr = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }
  else {
    if (anr[0] > anr[2]) {
      lanr = 0;
      a1 = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }

  // compute center point of incident face, in reference-face coordinates
  dVector3 center;
  if (nr[lanr] < 0) {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + dMUL(Sb[lanr],Rb[i*4+lanr]);
  }
  else {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - dMUL(Sb[lanr],Rb[i*4+lanr]);
  }

  // find the normal and non-normal axis numbers of the reference box
  int codeN,code1,code2;
  if (code <= 3) codeN = code-1; else codeN = code-4;
  if (codeN==0) {
    code1 = 1;
    code2 = 2;
  }
  else if (codeN==1) {
    code1 = 0;
    code2 = 2;
  }
  else {
    code1 = 0;
    code2 = 1;
  }

  // find the four corners of the incident face, in reference-face coordinates
  dReal quad[8];	// 2D coordinate of incident face (x,y pairs)
  dReal c1,c2,m11,m12,m21,m22;
  c1 = dDOT14 (center,Ra+code1);
  c2 = dDOT14 (center,Ra+code2);
  // optimize this? - we have already computed this data above, but it is not
  // stored in an easy-to-index format. for now it's quicker just to recompute
  // the four dot products.
  m11 = dDOT44 (Ra+code1,Rb+a1);
  m12 = dDOT44 (Ra+code1,Rb+a2);
  m21 = dDOT44 (Ra+code2,Rb+a1);
  m22 = dDOT44 (Ra+code2,Rb+a2);
  {
    dReal k1 = dMUL(m11,Sb[a1]);
    dReal k2 = dMUL(m21,Sb[a1]);
    dReal k3 = dMUL(m12,Sb[a2]);
    dReal k4 = dMUL(m22,Sb[a2]);
    quad[0] = c1 - k1 - k3;
    quad[1] = c2 - k2 - k4;
    quad[2] = c1 - k1 + k3;
    quad[3] = c2 - k2 + k4;
    quad[4] = c1 + k1 + k3;
    quad[5] = c2 + k2 + k4;
    quad[6] = c1 + k1 - k3;
    quad[7] = c2 + k2 - k4;
  }

  // find the size of the reference face
  dReal rect[2];
  rect[0] = Sa[code1];
  rect[1] = Sa[code2];

  // intersect the incident and reference faces
  dReal ret[16];
  int n = intersectRectQuad (rect,quad,ret);
  if (n < 1) return 0;		// this should never happen

  // convert the intersection points into reference-face coordinates,
  // and compute the contact position and depth for each point. only keep
  // those points that have a positive (penetrating) depth. delete points in
  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  dReal point[3*8];		// penetrating contact points
  dReal dep[8];			// depths for those points
  dReal det1 = dRecip(dMUL(m11,m22) - dMUL(m12,m21));
  m11 = dMUL(m11,det1);
  m12 = dMUL(m12,det1);
  m21 = dMUL(m21,det1);
  m22 = dMUL(m22,det1);
  int cnum = 0;			// number of penetrating contact points found
  for (j=0; j < n; j++) {
    dReal k1 =  dMUL(m22,(ret[j*2]-c1)) - dMUL(m12,(ret[j*2+1]-c2));
    dReal k2 = -dMUL(m21,(ret[j*2]-c1)) + dMUL(m11,(ret[j*2+1]-c2));
    for (i=0; i<3; i++) point[cnum*3+i] =
			  center[i] + dMUL(k1,Rb[i*4+a1]) + dMUL(k2,Rb[i*4+a2]);
    dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
    if (dep[cnum] >= 0) {
      ret[cnum*2] = ret[j*2];
      ret[cnum*2+1] = ret[j*2+1];
      cnum++;
    }
  }
  if (cnum < 1) return 0;	// this should never happen

  // we can't generate more contacts than we actually have
  if (maxc > cnum) maxc = cnum;
  if (maxc < 1) maxc = 1;

  if (cnum <= maxc) {
    // we have less contacts than we need, so we use them all
    for (j=0; j < cnum; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
      con->depth = dep[j];
    }
  }
  else {
    // we have more contacts than are wanted, some of them must be culled.
    // find the deepest point, it is always the first contact.
    int i1 = 0;
    dReal maxdepth = dep[0];
    for (i=1; i<cnum; i++) {
      if (dep[i] > maxdepth) {
	maxdepth = dep[i];
	i1 = i;
      }
    }

    int iret[8];
    cullPoints (cnum,ret,maxc,i1,iret);

    for (j=0; j < maxc; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
      con->depth = dep[iret[j]];
    }
    cnum = maxc;
  }

  *return_code = code;
  return cnum;
}
コード例 #13
0
void JitArm::fcmpo(UGeckoInstruction inst)
{
	INSTRUCTION_START
	JITDISABLE(bJITFloatingPointOff)
	u32 a = inst.FA, b = inst.FB;
	int cr = inst.CRFD;

	ARMReg vA = fpr.R0(a);
	ARMReg vB = fpr.R0(b);
	ARMReg fpscrReg = gpr.GetReg();
	ARMReg crReg = gpr.GetReg();
	Operand2 FPRFMask(0x1F, 0xA); // 0x1F000
	Operand2 LessThan(0x8, 0xA); // 0x8000
	Operand2 GreaterThan(0x4, 0xA); // 0x4000
	Operand2 EqualTo(0x2, 0xA); // 0x2000
	Operand2 NANRes(0x1, 0xA); // 0x1000
	FixupBranch Done1, Done2, Done3;
	LDR(fpscrReg, R9, PPCSTATE_OFF(fpscr));
	BIC(fpscrReg, fpscrReg, FPRFMask);

	VCMPE(vA, vB);
	VMRS(_PC);
	SetCC(CC_LT);
		ORR(fpscrReg, fpscrReg, LessThan);
		MOV(crReg,  8);
		Done1 = B();
	SetCC(CC_GT);
		ORR(fpscrReg, fpscrReg, GreaterThan);
		MOV(crReg,  4);
		Done2 = B();
	SetCC(CC_EQ);
		ORR(fpscrReg, fpscrReg, EqualTo);
		MOV(crReg,  2);
		Done3 = B();
	SetCC();

	ORR(fpscrReg, fpscrReg, NANRes);
	MOV(crReg,  1);

	VCMPE(vA, vA);
	VMRS(_PC);
	FixupBranch NanA = B_CC(CC_NEQ);
	VCMPE(vB, vB);
	VMRS(_PC);
	FixupBranch NanB = B_CC(CC_NEQ);

	SetFPException(fpscrReg, FPSCR_VXVC);
	FixupBranch Done4 = B();

	SetJumpTarget(NanA);
	SetJumpTarget(NanB);

	SetFPException(fpscrReg, FPSCR_VXSNAN);

	TST(fpscrReg, VEMask);

	FixupBranch noVXVC = B_CC(CC_NEQ);
	SetFPException(fpscrReg, FPSCR_VXVC);

	SetJumpTarget(noVXVC);
	SetJumpTarget(Done1);
	SetJumpTarget(Done2);
	SetJumpTarget(Done3);
	SetJumpTarget(Done4);
	STRB(crReg, R9, PPCSTATE_OFF(cr_fast) + cr);
	STR(fpscrReg, R9, PPCSTATE_OFF(fpscr));
	gpr.Unlock(fpscrReg, crReg);
}
コード例 #14
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x04(void) {		/* 0xED 0x04 : TST  B */
	TST(B);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #15
0
ファイル: decoder.c プロジェクト: CamiloARC/ARM_Cortex_M0
void decodeInstruction(instruction_t instruction,uint32_t *registro,flags_t *bandera, uint8_t *SRAM, uint16_t *codificacion, char **Flash)
{
    int i;
    *codificacion=0;    // valor incial
	//	comparar el mnemonic con el nombre de cada una de las funciones, y asi ejecutar la adecuada
	if( strcmp(instruction.mnemonic,"LDR") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            *codificacion=(13<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            instruction.op3_value<<=2;
            if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
            {
                LDR(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,SRAM);
            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
        {
            *codificacion=(19<<11)+(instruction.op1_value<<8)+instruction.op3_value;
            instruction.op3_value<<=2;
            if(((*(registro+13)+instruction.op3_value)>=0x20000000)&&((*(registro+13)+instruction.op3_value)<0x40000000))
            {
                LDR(registro+instruction.op1_value,*(registro+13),instruction.op3_value,SRAM);
            }
            if((*(registro+13)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+13)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+13)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='P') && (instruction.op3_type=='#')) // label
        {
            *codificacion=(9<<11)+(instruction.op1_value<<8)+instruction.op3_value;
            instruction.op3_value<<=2;
            if(((*(registro+15)+instruction.op3_value)>=0x20000000)&&((*(registro+15)+instruction.op3_value)<0x40000000))
            {
                LDR(registro+instruction.op1_value,*(registro+15),instruction.op3_value,SRAM);
            }
            if((*(registro+15)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+15)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+15)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            *codificacion=(11<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
            {
                LDR(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
            }
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"LDRB") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            *codificacion=(15<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
            {
                LDRB(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,SRAM);
            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
            {
                uint8_t data;
                IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,&data,Read);
                *(registro+instruction.op1_value)= (uint32_t)data;
            }

        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            *codificacion=(1<<14)+(7<<10)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
            {
                LDRB(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
            {
                uint8_t data;
                IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,&data,Read);
                *(registro+instruction.op1_value)=(uint32_t) data;
            }
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"LDRH") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            *codificacion=(1<<15)+(1<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            instruction.op3_value<<=1;
            if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
            {
                LDRH(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,SRAM);
            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Read);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            *codificacion=(5<<12)+(5<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
            {
                LDRH(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
            }
            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
            }
            registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"LDRSB") ==0)
    {
        *codificacion=(5<<12)+(3<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
        {
            LDRSB(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
        }
        if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
        {

        }
        if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
        {
            //IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"LDRSH") ==0)
    {
        *codificacion=(5<<12)+(7<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
        {
            LDRSH(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
        }
        if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
        {

        }
        if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
        {
            //IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Read);
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"STR") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            *codificacion=(3<<13)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            instruction.op3_value<<=2;
            if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
            {
                STR(*(registro+instruction.op1_value),*(registro+instruction.op2_value),instruction.op3_value,SRAM);
            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Write);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
        {
            *codificacion=(9<<12)+(instruction.op1_value<<8)+instruction.op3_type;
            instruction.op3_value<<=2;
            if(((*(registro+13)+instruction.op3_value)>=0x20000000)&&((*(registro+13)+instruction.op3_value)<0x40000000))
            {
                STR(*(registro+instruction.op1_value),*(registro+13),instruction.op3_value,SRAM);
            }
            if((*(registro+13)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+13)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+13)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Write);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            *codificacion=(5<<12)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
            {
                STR(*(registro+instruction.op1_value),*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Write);
            }
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"STRB") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            *codificacion=(7<<12)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
            {
                STRB(*(registro+instruction.op1_value),*(registro+instruction.op2_value),instruction.op3_value,SRAM);
            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
            {
                uint8_t data;
                data=(uint8_t)(*(registro+instruction.op1_value));
                IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,&data,Write);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            *codificacion=(21<<10)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
            {
                STRB(*(registro+instruction.op1_value),*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
            {
                uint8_t data;
                data=(uint8_t)(*(registro+instruction.op1_value));
                IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,&data,Write);
            }
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"STRH") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            *codificacion=(1<<15)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            instruction.op3_value<<=1;
            if(((*(registro+instruction.op2_value)+instruction.op3_value)>=0x20000000)&&((*(registro+instruction.op2_value)+instruction.op3_value)<0x40000000))
            {
                STRH(*(registro+instruction.op1_value),*(registro+instruction.op2_value),instruction.op3_value,SRAM);
            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+instruction.op3_value)>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+instruction.op3_value)&0xFF,registro+instruction.op1_value,Write);
            }
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            *codificacion=(5<<12)+(1<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
            if(((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x20000000)&&((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x40000000))
            {
                STRH(*(registro+instruction.op1_value),*(registro+instruction.op2_value),*(registro+instruction.op3_value),SRAM);
            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))<0x20000000)
            {

            }
            if((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))>=0x40000000)
            {
                //IOAccess((*(registro+instruction.op2_value)+*(registro+instruction.op3_value))&0xFF,registro+instruction.op1_value,Write);
            }
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"PUSH") ==0)
    {
        for(i=0;i<8;i++)
        {
            *codificacion+=(instruction.registers_list[i]<<i);
        }
        *codificacion+=(11<<12)+(1<<10)+(instruction.registers_list[14]<<8);
        PUSH(registro,SRAM,&instruction.registers_list[0]);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"POP") ==0)
    {
        for(i=0;i<8;i++)
        {
            *codificacion+=(instruction.registers_list[i]<<i);
        }
        *codificacion=(11<<12)+(3<<10)+(instruction.registers_list[15]<<8);
        POP(registro,SRAM,&instruction.registers_list[0]);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"ADCS") ==0)
    {
        *codificacion=(1<<14)+(5<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        ADCS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
        registro[15]++;
	}
	if( strcmp(instruction.mnemonic,"ADD") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
        {
            ADD(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value));
            *codificacion=(1<<14)+(1<<10)+(instruction.op2_value<<3)+((8&instruction.op1_value)<<4)+(7&instruction.op1_value);
        }
         if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
        {
            ADD(registro+instruction.op1_value,*(registro+13),instruction.op2_value);
            *codificacion=(21<<11)+(instruction.op1_value<<8)+instruction.op3_value;
        }
        if((instruction.op1_type=='S') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
        {
            ADD(registro+13,*(registro+13),instruction.op3_value);
            *codificacion=(11<<12)+instruction.op3_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='S') && (instruction.op3_type=='R'))
        {
            ADD(registro+instruction.op1_value,*(registro+13),*(registro+instruction.op3_value));
            *codificacion=(1<<14)+(1<<10)+(13<<3)+((8&instruction.op1_value)<<4)+(7&instruction.op1_value);
        }
        if((instruction.op1_type=='S') && (instruction.op2_type=='R'))
        {
            ADD(registro+13,*(registro+13),*(registro+instruction.op2_value));
            *codificacion=(1<<14)+(9<<7)+5+(instruction.op2_value<<3);
        }
        registro[15]++;
    }
	if( strcmp(instruction.mnemonic,"ADDS") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
        {
            ADDS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
            *codificacion=(7<<10)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
        {
            ADDS(registro+instruction.op1_value,*(registro+instruction.op1_value),instruction.op2_value,bandera);
            *codificacion=(3<<12)+(instruction.op1_value<<8)+instruction.op2_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
        {
            ADDS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
            *codificacion=(3<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
	}
	// los parametros de las demas funciones aritmeticas, de desplazamiento y logicas son similares
    if( strcmp(instruction.mnemonic,"ANDS") ==0)
    {
        *codificacion=(1<<14)+(instruction.op2_value<<3)+instruction.op1_value;
        ANDS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"ASRS") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
		{
			ASRS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
			*codificacion=(1<<14)+(1<<8)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
		{
			ASRS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
            *codificacion=(1<<12)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"BICS") ==0)
    {
        *codificacion=(1<<14)+(14<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        BICS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"CMN") ==0)
    {
        *codificacion=(1<<14)+(11<6)+(instruction.op2_value<<3)+instruction.op1_value;
        CMN(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera); //	En diferencia a las demas funciones, se envian como parametros 2 valores
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"CMP") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
		{
		    if(instruction.op1_value>=8)
		    {
                CMP(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);	//	En diferencia a las demas funciones, se envian como parametros 2 valores
                *codificacion=(1<<14)+(5<<8)+(instruction.op2_value<<3)+(7&instruction.op1_value)+((8&instruction.op1_value)<<4);
		    }
		    else
		    {
                CMP(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);	//	En diferencia a las demas funciones, se envian como parametros 2 valores
                *codificacion=(1<<14)+(10<<6)+(instruction.op2_value<<3)+instruction.op1_value;
		    }
		}
        if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
		{
			CMP(*(registro+instruction.op1_value),instruction.op2_value, bandera);	// Como parametros se tienen el contenido de un registro y un valor
            *codificacion=(5<<11)+(instruction.op1_value<<8)+instruction.op2_value;
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"EORS") ==0)
    {
        *codificacion=(1<<14)+(1<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        EORS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value),bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"LSLS") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
		{
			LSLS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
			*codificacion=(1<<14)+(1<<7)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
		{
			LSLS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
            *codificacion=(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
	}
    if( strcmp(instruction.mnemonic,"LSRS") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
		{
	        LSRS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
	        *codificacion=(1<<14)+(3<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
		{
			LSRS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
			*codificacion=(1<<11)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
	}
    if( strcmp(instruction.mnemonic,"MOV") ==0)
    {
        *codificacion=(1<<14)+(3<<9)+(instruction.op2_value<<3)+(7&instruction.op1_value)+((8&instruction.op1_value)<<4);
        MOV(registro+instruction.op1_value,*(registro+instruction.op2_value));	//	Envio como parametros una direccion y el contenido de un registro
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"MOVS") ==0)
    {
		if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
		{
			MOVS(registro+instruction.op1_value,instruction.op2_value, bandera);
			*codificacion=(1<<13)+(instruction.op1_value<<8)+instruction.op2_value;
		}
		if((instruction.op1_type=='R') && (instruction.op2_type=='R'))
        {
            MOVS(registro+instruction.op1_value,*(registro+instruction.op2_value),bandera);
            *codificacion=(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"MULS") ==0)
    {
        *codificacion=(1<<14)+(13<<6)+(instruction.op2_value<<3)+instruction.op3_value;
        MULS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
        registro[15]++;
	}
	if( strcmp(instruction.mnemonic,"MVNS") ==0)
    {
        *codificacion=(1<<14)+(15<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        RSBS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"NOP") ==0)
    {
        *codificacion=(11<<12)+(15<<8);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"ORRS") ==0)
    {
        *codificacion=(1<<14)+(3<<8)+(instruction.op2_value<<3)+instruction.op1_value;
        ORRS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"REV") ==0)
    {
        *codificacion=(11<<12)+(5<<9)+(instruction.op2_value<<3)+instruction.op1_value;
        REV(registro+instruction.op1_value, *(registro+instruction.op2_value));
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"REV16") ==0)
    {
        *codificacion=(11<<12)+(5<<9)+(1<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        REV16(registro+instruction.op1_value,*(registro+instruction.op2_value));
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"REVSH") ==0)
    {
        *codificacion=(11<<12)+(5<<9)+(3<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        REVSH(registro+instruction.op1_value,*(registro+instruction.op2_value));
        registro[15]++;
    }
	if( strcmp(instruction.mnemonic,"RORS") ==0)
    {
        *codificacion=(1<<14)+(7<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        RORS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
        registro[15]++;
	}
    if( strcmp(instruction.mnemonic,"RSBS") ==0)
    {
        *codificacion=(1<<14)+(9<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        RSBS(registro+instruction.op1_value,*(registro+instruction.op2_value), bandera);
        registro[15]++;
    }
    if( strcmp(instruction.mnemonic,"SBCS") ==0)
    {
        *codificacion=(1<<14)+(3<<7)+(instruction.op2_value<<3)+instruction.op1_value;
        SBCS(registro+instruction.op1_value,*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);
        registro[15]++;
	}
	if( strcmp(instruction.mnemonic,"SUB") ==0)
    {
        if((instruction.op1_type=='S') && (instruction.op2_type=='S') && (instruction.op3_type=='#'))
        {
            SUB(registro+13,*(registro+13),instruction.op3_value);
            *codificacion=(11<<12)+(1<<7)+instruction.op3_value;
        }
        registro[15]++;
    }
	if( strcmp(instruction.mnemonic,"SUBS") ==0)
    {
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='#'))
		{
			SUBS(registro+instruction.op1_value,*(registro+instruction.op2_value),instruction.op3_value,bandera);
			*codificacion=(15<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='#'))
        {
            SUBS(registro+instruction.op1_value,*(registro+instruction.op1_value),instruction.op2_value,bandera);
            *codificacion=(7<<11)+(instruction.op1_value<<8)+instruction.op2_value;
        }
        if((instruction.op1_type=='R') && (instruction.op2_type=='R') && (instruction.op3_type=='R'))
		{
			SUBS(registro+instruction.op1_value,*(registro+instruction.op2_value),*(registro+instruction.op3_value), bandera);
			*codificacion=(13<<9)+(instruction.op3_value<<6)+(instruction.op2_value<<3)+instruction.op1_value;
        }
        registro[15]++;
	}
    if( strcmp(instruction.mnemonic,"TST") ==0)
    {
        *codificacion=(1<<14)+(1<<9)+(instruction.op2_value<<3)+instruction.op1_value;
        TST(*(registro+instruction.op1_value),*(registro+instruction.op2_value), bandera);	// Como parametros se tienen el contenido de un registro y un valor
        registro[15]++;
    }
	// Las siguientes funciones, son funciones de saltos
    if( strcmp(instruction.mnemonic,"B") ==0)
    {
        *codificacion=(7<<13)+instruction.op1_value;
        B(registro,instruction.op1_value);	// Envio como parametroa la direccion de registro y el valor del salto
    }
    if( strcmp(instruction.mnemonic,"BL") ==0)
    {
        *codificacion=(31<<11)+(2047&instruction.op1_value);
        BL(registro,instruction.op1_value);	// Envio como parametroa la direccion de registro y el valor del salto
    }
    if( strcmp(instruction.mnemonic,"BLX") ==0)
    {
        *codificacion=(1<<14)+(15<<7)+(instruction.op1_value<<3);
        BLX(registro,*(registro+instruction.op1_value));	// Envio como parametroa la direccion de registro y el contenido de un registro
    }
    if( strcmp(instruction.mnemonic,"BX") ==0)
    {
        *codificacion=(1<<14)+(14<<7)+(instruction.op1_value<<3);
        if(instruction.op1_type=='L')	//	Sucede cuando
        {
            BX(registro,registro[14]);  //  PC=LR
        }
        if(instruction.op1_type=='R')	// Sucede cuando se tiene como parametro un registro diferente a LR
        {
            BX(registro,*(registro+instruction.op1_value));
        }
    }
    if( strcmp(instruction.mnemonic,"BEQ") ==0)
    {
        *codificacion=(13<<12)+instruction.op1_value;
        BEQ(registro,instruction.op1_value,*bandera);	// Envio como parametros la direccion de registro, el valor del salto y las banderas
    }
	// Todas las siguientes funciones de salto tienen los mismos parametro que BEQ
    if( strcmp(instruction.mnemonic,"BNE") ==0)
    {
        *codificacion=(13<<12)+(1<<8)+instruction.op1_value;
        BNE(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BCS") ==0)
    {
        *codificacion=(13<<12)+(2<<8)+instruction.op1_value;
        BCS(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BCC") ==0)
    {
        *codificacion=(13<<12)+(3<<8)+instruction.op1_value;
        BCC(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BMI") ==0)
    {
        *codificacion=(13<<12)+(4<<8)+instruction.op1_value;
        BMI(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BPL") ==0)
    {
        *codificacion=(13<<12)+(5<<8)+instruction.op1_value;
        BPL(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BVS") ==0)
    {
        *codificacion=(13<<12)+(6<<8)+instruction.op1_value;
        BVS(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BVC") ==0)
    {
        *codificacion=(13<<12)+(7<<8)+instruction.op1_value;
        BVC(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BHI") ==0)
    {
        *codificacion=(13<<12)+(8<<8)+instruction.op1_value;
        BHI(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BLS") ==0)
    {
        *codificacion=(13<<12)+(9<<8)+instruction.op1_value;
        BLS(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BGE") ==0)
    {
        *codificacion=(13<<12)+(10<<8)+instruction.op1_value;
        BGE(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BLT") ==0)
    {
        *codificacion=(13<<12)+(11<<8)+instruction.op1_value;
        BLT(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BGT") ==0)
    {
        *codificacion=(13<<12)+(12<<8)+instruction.op1_value;
        BGT(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BLE") ==0)
    {
        *codificacion=(13<<12)+(13<<8)+instruction.op1_value;
        BLE(registro,instruction.op1_value,*bandera);
    }
    if( strcmp(instruction.mnemonic,"BAL") ==0)
    {
        *codificacion=(13<<12)+(14<<8)+instruction.op1_value;
        BAL(registro,instruction.op1_value,*bandera);
    }
}
コード例 #16
0
ファイル: tst_fmt_num.c プロジェクト: Brainiarc7/ralink_sdk
int tst(void)
{
  int ret = 0;
  int num = 0;
  
  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.0d", 0);
  TST_B_TST(ret, 1, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.d", 0);
  TST_B_TST(ret, 2, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%I.*d", 0, 0);
  TST_B_TST(ret, 3, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.0x", 0);
  TST_B_TST(ret, 4, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.x", 0);
  TST_B_TST(ret, 5, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.*x", 0, 0);
  TST_B_TST(ret, 6, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#.*x", 0, 0);
  TST_B_TST(ret, 7, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.0o", 0);
  TST_B_TST(ret, 8, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.o", 0);
  TST_B_TST(ret, 9, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%.*o", 0, 0);
  TST_B_TST(ret, 10, s1->len);

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#.o", 0);
  TST_B_TST(ret, 11, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "0"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#x", 1);
  TST_B_TST(ret, 12, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "0x1"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#.x", 1);
  TST_B_TST(ret, 13, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "0x1"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#x", 0);
  TST_B_TST(ret, 14, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "0"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#8o", 1);
  TST_B_TST(ret, 15, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "      01"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#08o", 0);
  TST_B_TST(ret, 16, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "00000000"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#8x", 1);
  TST_B_TST(ret, 17, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "     0x1"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%#08x", 1);
  TST_B_TST(ret, 18, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "0x000001"));

  vstr_del(s1, 1, s1->len);
  vstr_add_fmt(s1, 0, "%-*d", 4, 1);
  TST_B_TST(ret, 18, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, "1   "));

  sprintf(buf, "%#.o", 0);
  if (!buf[0])
    return (EXIT_FAILED_OK); /* Solaris (2.8) gets this wrong at least... */
  
  num = 5;
  while (num--)
  {
    unsigned int mfail_count = 0;
    int spaces = 2;
    
    while (spaces < 10)
    {
      const char *fmt = NULL;
      
#define FMT(x) (fmt = ((num == 0xff) ? "<%" x "x>" : (num == 0xfe) ? "<%" x "X>" : (num == 0777) ? "<%" x "o>" : "<%" x "d>"))
      
      FMT("+#*");
      vstr_del(s3, 1, s3->len);
      
      mfail_count = 0;
      do
      {
        ASSERT(!s3->len);
        vstr_free_spare_nodes(s3->conf, VSTR_TYPE_NODE_BUF, 1000);
        tst_mfail_num(++mfail_count);
      } while (!vstr_add_fmt(s3, s3->len, fmt, spaces, num));
      tst_mfail_num(0);
      sprintf(buf, fmt, spaces, num);

      TST_B_TST(ret, 19, !VSTR_CMP_CSTR_EQ(s3, 1, s3->len, buf));

      FMT("*");
      vstr_del(s3, 1, s3->len);
      
      mfail_count = 0;
      do
      {
        ASSERT(!s3->len);
        vstr_free_spare_nodes(s3->conf, VSTR_TYPE_NODE_BUF, 1000);
        tst_mfail_num(++mfail_count);
      } while (!vstr_add_fmt(s3, s3->len, fmt, -spaces, num));
      tst_mfail_num(0);
      sprintf(buf, fmt, -spaces, num);

      TST_B_TST(ret, 19, !VSTR_CMP_CSTR_EQ(s3, 1, s3->len, buf));

      FMT(" #.*");
      vstr_del(s3, 1, s3->len);
      
      mfail_count = 0;
      do
      {
        ASSERT(!s3->len);
        vstr_free_spare_nodes(s3->conf, VSTR_TYPE_NODE_BUF, 1000);
        tst_mfail_num(++mfail_count);
      } while (!vstr_add_fmt(s3, s3->len, fmt, spaces, num));
      tst_mfail_num(0);
      sprintf(buf, fmt, spaces, num);

      TST_B_TST(ret, 19, !VSTR_CMP_CSTR_EQ(s3, 1, s3->len, buf));

      ++spaces;
    }

    switch (num)
    {
      case    4: num =    -3; break;
      case   -4: num = 0x100; break;
      case 0xff:              break;
      case 0xfe: num = 01000; break;
      case 0777: num =     1; break;
      case    0:              break;
    }
  }
  
  {
    static const char fmts[][80] = {
     "<%o|%#o|%d|%x|%#x|%#d>",
     "<%0o|%0#o|%0d|%0x|%0#x|%0#d>",
     "<%.o|%#.o|%.d|%.x|%#.x|%#.d>",
     "<%0.o|%0#.o|%0.d|%0.x|%0#.x|%0#.d>",
     "<%.1o|%#.1o|%.1d|%.1x|%#.1x|%#.1d>",
     "<%0.1o|%0#.1o|%0.1d|%0.1x|%0#.1x|%0#.1d>",
     "<%8o|%#8o|%8d|%8x|%#8x|%#8d>",
     "<%08o|%#08o|%08d|%08x|%0#8x|%0#8d>",
     "<%8.o|%#8.o|%8.d|%8.x|%#8.x|%#8.d>",
     "<%08.o|%#08.o|%08.d|%08.x|%0#8.x|%0#8.d>",
     "<%4.1o|%#4.1o|%4.1d|%4.1x|%#4.1x|%#4.1d>",
     "<%4.8o|%#4.8o|%4.8d|%4.8x|%#4.8x|%#4.8d>",
     "<%8.4o|%#8.4o|%8.4d|%8.4x|%#8.4x|%#8.4d>",
     "<%04.1o|%0#4.1o|%04.1d|%04.1x|%0#4.1x|%0#4.1d>",
     "<%04.8o|%0#4.8o|%04.8d|%04.8x|%0#4.8x|%0#4.8d>",
     "<%08.4o|%0#8.4o|%08.4d|%08.4x|%0#8.4x|%0#8.4d>",
     "<%-o|%-#o|%-d|%-x|%-#x|%-#d>",
     "<%-0o|%-0#o|%-0d|%-0x|%-0#x|%-0#d>",
     "<%-.o|%-#.o|%-.d|%-.x|%-#.x|%-#.d>",
     "<%-0.o|%-0#.o|%-0.d|%-0.x|%-0#.x|%-0#.d>",
     "<%-.1o|%-#.1o|%-.1d|%-.1x|%-#.1x|%-#.1d>",
     "<%-0.1o|%-0#.1o|%-0.1d|%-0.1x|%-0#.1x|%-0#.1d>",
     "<%-8o|%-#8o|%-8d|%-8x|%-#8x|%-#8d>",
     "<%-08o|%-#08o|%-08d|%-08x|%-0#8x|%-0#8d>",
     "<%-8.o|%-#8.o|%-8.d|%-8.x|%-#8.x|%-#8.d>",
     "<%-08.o|%-#08.o|%-08.d|%-08.x|%-0#8.x|%-0#8.d>",
     "<%-4.1o|%-#4.1o|%-4.1d|%-4.1x|%-#4.1x|%-#4.1d>",
     "<%-4.8o|%-#4.8o|%-4.8d|%-4.8x|%-#4.8x|%-#4.8d>",
     "<%-8.4o|%-#8.4o|%-8.4d|%-8.4x|%-#8.4x|%-#8.4d>",
     "<%-04.1o|%-0#4.1o|%-04.1d|%-04.1x|%-0#4.1x|%-0#4.1d>",
     "<%-04.8o|%-0#4.8o|%-04.8d|%-04.8x|%-0#4.8x|%-0#4.8d>",
     "<%-08.4o|%-0#8.4o|%-08.4d|%-08.4x|%-0#8.4x|%-0#8.4d>",
     "<%+o|%+#o|%+d|%+x|%+#x|%+#d>",
     "<%+0o|%+0#o|%+0d|%+0x|%+0#x|%+0#d>",
     "<%+.o|%+#.o|%+.d|%+.x|%+#.x|%+#.d>",
     "<%+0.o|%+0#.o|%+0.d|%+0.x|%+0#.x|%+0#.d>",
     "<%+.1o|%+#.1o|%+.1d|%+.1x|%+#.1x|%+#.1d>",
     "<%+0.1o|%+0#.1o|%+0.1d|%+0.1x|%+0#.1x|%+0#.1d>",
     "<%+8o|%+#8o|%+8d|%+8x|%+#8x|%+#8d>",
     "<%+08o|%+#08o|%+08d|%+08x|%+0#8x|%+0#8d>",
     "<%+8.o|%+#8.o|%+8.d|%+8.x|%+#8.x|%+#8.d>",
     "<%+08.o|%+#08.o|%+08.d|%+08.x|%+0#8.x|%+0#8.d>",
     "<%+4.1o|%+#4.1o|%+4.1d|%+4.1x|%+#4.1x|%+#4.1d>",
     "<%+4.8o|%+#4.8o|%+4.8d|%+4.8x|%+#4.8x|%+#4.8d>",
     "<%+8.4o|%+#8.4o|%+8.4d|%+8.4x|%+#8.4x|%+#8.4d>",
     "<%+04.1o|%+0#4.1o|%+04.1d|%+04.1x|%+0#4.1x|%+0#4.1d>",
     "<%+04.8o|%+0#4.8o|%+04.8d|%+04.8x|%+0#4.8x|%+0#4.8d>",
     "<%+08.4o|%+0#8.4o|%+08.4d|%+08.4x|%+0#8.4x|%+0#8.4d>",
     "<% o|% #o|% d|% x|% #x|% #d>",
     "<% 0o|% 0#o|% 0d|% 0x|% 0#x|% 0#d>",
     "<% .o|% #.o|% .d|% .x|% #.x|% #.d>",
     "<% 0.o|% 0#.o|% 0.d|% 0.x|% 0#.x|% 0#.d>",
     "<% .1o|% #.1o|% .1d|% .1x|% #.1x|% #.1d>",
     "<% 0.1o|% 0#.1o|% 0.1d|% 0.1x|% 0#.1x|% 0#.1d>",
     "<% 8o|% #8o|% 8d|% 8x|% #8x|% #8d>",
     "<% 08o|% #08o|% 08d|% 08x|% 0#8x|% 0#8d>",
     "<% 8.o|% #8.o|% 8.d|% 8.x|% #8.x|% #8.d>",
     "<% 08.o|% #08.o|% 08.d|% 08.x|% 0#8.x|% 0#8.d>",
     "<% 4.1o|% #4.1o|% 4.1d|% 4.1x|% #4.1x|% #4.1d>",
     "<% 4.8o|% #4.8o|% 4.8d|% 4.8x|% #4.8x|% #4.8d>",
     "<% 8.4o|% #8.4o|% 8.4d|% 8.4x|% #8.4x|% #8.4d>",
     "<% 04.1o|% 0#4.1o|% 04.1d|% 04.1x|% 0#4.1x|% 0#4.1d>",
     "<% 04.8o|% 0#4.8o|% 04.8d|% 04.8x|% 0#4.8x|% 0#4.8d>",
     "<% 08.4o|% 0#8.4o|% 08.4d|% 08.4x|% 0#8.4x|% 0#8.4d>"
    };

#define TST(sym, fmt, val) \
	tst_ ## sym (fmt, \
	             (val), (val), (val), (val), (val), (val))

    unsigned int count = 0;

    while (count < 0x1002)
    {
      unsigned int scan = 0;

      while (scan < sizeof(fmts)/sizeof(fmts[0]))
      {
        TST(host, fmts[scan], count);
        TST(vstr, fmts[scan], count);

        TST_B_TST(ret, 30, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, buf));

        TST(host, fmts[scan], -(int)count);
        TST(vstr, fmts[scan], -(int)count);

        TST_B_TST(ret, 30, !VSTR_CMP_CSTR_EQ(s1, 1, s1->len, buf));

        ++scan;
      }

      switch (count)
      { case 0x0000: count = 0x0001; break;
        case 0x0001: count =     10; break;
        case     10: count = 0x0010; break;
        case 0x0010: count =    100; break;
        case    100: count = 0x0100; break;
        case 0x0100: count =   1000; break;
        case   1000: count = 0x1000; break;
        case 0x1000: count = 0x1001; break;
        case 0x1001: count = 0x10000001; break;
        case 0x10000001: count = 0x10000002; break;
        default: abort();
      }
    }
  }

  return (TST_B_RET(ret));
}
コード例 #17
0
ファイル: main.cpp プロジェクト: ForwardFunk/SynTree
int main(int argc, char ** argv) {
    memory::initialize(0);
    bool do_display_usage = false;
    bool test_all = false;
    parse_cmd_line_args(argc, argv, do_display_usage, test_all);
    TST(random);
    TST(vector);
    TST(symbol_table);
    TST(region);
    TST(symbol);
    TST(heap);
    TST(hashtable);
    TST(rational);
    TST(inf_rational);
    TST(ast);
    TST(optional);
    TST(bit_vector);
    TST(fixed_bit_vector);
    TST(tbv);
    TST(doc);
    TST(udoc_relation);
    TST(string_buffer);
    TST(map);
    TST(diff_logic);
    TST(uint_set);
    TST_ARGV(expr_rand);
    TST(list);
    TST(small_object_allocator);
    TST(timeout);
    TST(proof_checker);
    TST(simplifier);
    TST(bv_simplifier_plugin);
    TST(bit_blaster);
    TST(var_subst);
    TST(simple_parser);
    TST(api);
    TST(old_interval);
    TST(get_implied_equalities);
    TST(arith_simplifier_plugin);
    TST(matcher);
    TST(object_allocator);
    TST(mpz);
    TST(mpq);
    TST(mpf);
    TST(total_order);
    TST(dl_table);
    TST(dl_context);
    TST(dl_util);
    TST(dl_product_relation);
    TST(dl_relation);
    TST(parray);
    TST(stack);
    TST(escaped);
    TST(buffer);
    TST(chashtable);
    TST(ex);
    TST(nlarith_util);
    TST(api_bug);
    TST(arith_rewriter);
    TST(check_assumptions);
    TST(smt_context);
    TST(theory_dl);
    TST(model_retrieval);
    TST(model_based_opt);
    TST(factor_rewriter);
    TST(smt2print_parse);
    TST(substitution);
    TST(polynomial);
    TST(upolynomial);
    TST(algebraic);
    TST(polynomial_factorization);
    TST(prime_generator);
    TST(permutation);
    TST(nlsat);
    TST(ext_numeral);
    TST(interval);
    TST(f2n);
    TST(hwf);
    TST(trigo);
    TST(bits);
    TST(mpbq);
    TST(mpfx);
    TST(mpff);
    TST(horn_subsume_model_converter);
    TST(model2expr);
    TST(hilbert_basis);
    TST(heap_trie);
    TST(karr);
    TST(no_overflow);
    TST(memory);
    TST(datalog_parser);
    TST_ARGV(datalog_parser_file);
    TST(dl_query);
    TST(quant_solve);
    TST(rcf);
    TST(polynorm);
    TST(qe_arith);
    TST(expr_substitution);
    TST(sorting_network);
    TST(theory_pb);
    TST(simplex);
    TST(sat_user_scope);
    TST(pdr);
    TST_ARGV(ddnf);
    TST(model_evaluator);
    //TST_ARGV(hs);
}
コード例 #18
0
ファイル: box.cpp プロジェクト: JdeRobot/ThirdParty
int dBoxBox (const dVector3 p1, const dMatrix3 R1,
	     const dVector3 side1, const dVector3 p2,
	     const dMatrix3 R2, const dVector3 side2,
	     dVector3 normal, dReal *depth, int *return_code,
	     int flags, dContactGeom *contact, int skip)
{
  const dReal fudge_factor = REAL(1.05);
  dVector3 p,pp,normalC={0,0,0};
  const dReal *normalR = 0;
  dReal A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
    Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,expr1_val;
  int i,j,invert_normal,code;

  // get vector from centers of box 1 to box 2, relative to box 1
  p[0] = p2[0] - p1[0];
  p[1] = p2[1] - p1[1];
  p[2] = p2[2] - p1[2];
  dMultiply1_331 (pp,R1,p);		// get pp = p relative to body 1

  // get side lengths / 2
  A[0] = side1[0]*REAL(0.5);
  A[1] = side1[1]*REAL(0.5);
  A[2] = side1[2]*REAL(0.5);
  B[0] = side2[0]*REAL(0.5);
  B[1] = side2[1]*REAL(0.5);
  B[2] = side2[2]*REAL(0.5);

  // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
  R11 = dCalcVectorDot3_44(R1+0,R2+0); R12 = dCalcVectorDot3_44(R1+0,R2+1); R13 = dCalcVectorDot3_44(R1+0,R2+2);
  R21 = dCalcVectorDot3_44(R1+1,R2+0); R22 = dCalcVectorDot3_44(R1+1,R2+1); R23 = dCalcVectorDot3_44(R1+1,R2+2);
  R31 = dCalcVectorDot3_44(R1+2,R2+0); R32 = dCalcVectorDot3_44(R1+2,R2+1); R33 = dCalcVectorDot3_44(R1+2,R2+2);

  Q11 = dFabs(R11); Q12 = dFabs(R12); Q13 = dFabs(R13);
  Q21 = dFabs(R21); Q22 = dFabs(R22); Q23 = dFabs(R23);
  Q31 = dFabs(R31); Q32 = dFabs(R32); Q33 = dFabs(R33);

  // for all 15 possible separating axes:
  //   * see if the axis separates the boxes. if so, return 0.
  //   * find the depth of the penetration along the separating axis (s2)
  //   * if this is the largest depth so far, record it.
  // the normal vector will be set to the separating axis with the smallest
  // depth. note: normalR is set to point to a column of R1 or R2 if that is
  // the smallest depth normal so far. otherwise normalR is 0 and normalC is
  // set to a vector relative to body 1. invert_normal is 1 if the sign of
  // the normal should be flipped.

  do {
#define TST(expr1,expr2,norm,cc) \
    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
    s2 = dFabs(expr1_val) - (expr2); \
    if (s2 > 0) return 0; \
    if (s2 > s) { \
      s = s2; \
      normalR = norm; \
      invert_normal = ((expr1_val) < 0); \
      code = (cc); \
	  if (flags & CONTACTS_UNIMPORTANT) break; \
	}

    s = -dInfinity;
    invert_normal = 0;
    code = 0;

    // separating axis = u1,u2,u3
    TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
    TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
    TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);

    // separating axis = v1,v2,v3
    TST (dCalcVectorDot3_41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
    TST (dCalcVectorDot3_41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
    TST (dCalcVectorDot3_41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);

    // note: cross product axes need to be scaled when s is computed.
    // normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
    expr1_val = (expr1); /* Avoid duplicate evaluation of expr1 */ \
    s2 = dFabs(expr1_val) - (expr2); \
    if (s2 > 0) return 0; \
    l = dSqrt ((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
    if (l > 0) { \
      s2 /= l; \
      if (s2*fudge_factor > s) { \
        s = s2; \
        normalR = 0; \
        normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
        invert_normal = ((expr1_val) < 0); \
        code = (cc); \
        if (flags & CONTACTS_UNIMPORTANT) break; \
	  } \
	}

    // We only need to check 3 edges per box 
    // since parallel edges are equivalent.

    // separating axis = u1 x (v1,v2,v3)
    TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
    TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
    TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);

    // separating axis = u2 x (v1,v2,v3)
    TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
    TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
    TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);

    // separating axis = u3 x (v1,v2,v3)
    TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
    TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
    TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);
#undef TST
  } while (0);

  if (!code) return 0;

  // if we get to this point, the boxes interpenetrate. compute the normal
  // in global coordinates.
  if (normalR) {
    normal[0] = normalR[0];
    normal[1] = normalR[4];
    normal[2] = normalR[8];
  }
  else {
    dMultiply0_331 (normal,R1,normalC);
  }
  if (invert_normal) {
    normal[0] = -normal[0];
    normal[1] = -normal[1];
    normal[2] = -normal[2];
  }
  *depth = -s;

  // compute contact point(s)

  if (code > 6) {
    // An edge from box 1 touches an edge from box 2.
    // find a point pa on the intersecting edge of box 1
    dVector3 pa;
    dReal sign;
    // Copy p1 into pa
    for (i=0; i<3; i++) pa[i] = p1[i]; // why no memcpy?
    // Get world position of p2 into pa
    for (j=0; j<3; j++) {
      sign = (dCalcVectorDot3_14(normal,R1+j) > 0) ? REAL(1.0) : REAL(-1.0);
      for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
    }

    // find a point pb on the intersecting edge of box 2
    dVector3 pb;
    // Copy p2 into pb
    for (i=0; i<3; i++) pb[i] = p2[i]; // why no memcpy?
    // Get world position of p2 into pb
    for (j=0; j<3; j++) {
      sign = (dCalcVectorDot3_14(normal,R2+j) > 0) ? REAL(-1.0) : REAL(1.0);
      for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
    }
    
    dReal alpha,beta;
    dVector3 ua,ub;
    // Get direction of first edge
    for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
    // Get direction of second edge
    for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];
    // Get closest points between edges (one at each)
    dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);    
    for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
    for (i=0; i<3; i++) pb[i] += ub[i]*beta;
    // Set the contact point as halfway between the 2 closest points
    for (i=0; i<3; i++) contact[0].pos[i] = REAL(0.5)*(pa[i]+pb[i]);
    contact[0].depth = *depth;
    *return_code = code;
    return 1;
  }

  // okay, we have a face-something intersection (because the separating
  // axis is perpendicular to a face). define face 'a' to be the reference
  // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
  // the incident face (the closest face of the other box).
  // Note: Unmodified parameter values are being used here
  const dReal *Ra,*Rb,*pa,*pb,*Sa,*Sb;
  if (code <= 3) { // One of the faces of box 1 is the reference face
    Ra = R1; // Rotation of 'a'
    Rb = R2; // Rotation of 'b'
    pa = p1; // Center (location) of 'a'
    pb = p2; // Center (location) of 'b'
    Sa = A;  // Side Lenght of 'a'
    Sb = B;  // Side Lenght of 'b'
  }
  else { // One of the faces of box 2 is the reference face
    Ra = R2; // Rotation of 'a'
    Rb = R1; // Rotation of 'b'
    pa = p2; // Center (location) of 'a'
    pb = p1; // Center (location) of 'b'
    Sa = B;  // Side Lenght of 'a'
    Sb = A;  // Side Lenght of 'b'
  }

  // nr = normal vector of reference face dotted with axes of incident box.
  // anr = absolute values of nr.
  /*
	The normal is flipped if necessary so it always points outward from box 'a',
	box 'b' is thus always the incident box
  */
  dVector3 normal2,nr,anr;
  if (code <= 3) {
    normal2[0] = normal[0];
    normal2[1] = normal[1];
    normal2[2] = normal[2];
  }
  else {
    normal2[0] = -normal[0];
    normal2[1] = -normal[1];
    normal2[2] = -normal[2];
  }
  // Rotate normal2 in incident box opposite direction
  dMultiply1_331 (nr,Rb,normal2);
  anr[0] = dFabs (nr[0]);
  anr[1] = dFabs (nr[1]);
  anr[2] = dFabs (nr[2]);

  // find the largest compontent of anr: this corresponds to the normal
  // for the incident face. the other axis numbers of the incident face
  // are stored in a1,a2.
  int lanr,a1,a2;
  if (anr[1] > anr[0]) {
    if (anr[1] > anr[2]) {
      a1 = 0;
      lanr = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }
  else {
    if (anr[0] > anr[2]) {
      lanr = 0;
      a1 = 1;
      a2 = 2;
    }
    else {
      a1 = 0;
      a2 = 1;
      lanr = 2;
    }
  }

  // compute center point of incident face, in reference-face coordinates
  dVector3 center;
  if (nr[lanr] < 0) {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
  }
  else {
    for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
  }

  // find the normal and non-normal axis numbers of the reference box
  int codeN,code1,code2;
  if (code <= 3) codeN = code-1; else codeN = code-4;
  if (codeN==0) {
    code1 = 1;
    code2 = 2;
  }
  else if (codeN==1) {
    code1 = 0;
    code2 = 2;
  }
  else {
    code1 = 0;
    code2 = 1;
  }

  // find the four corners of the incident face, in reference-face coordinates
  dReal quad[8];	// 2D coordinate of incident face (x,y pairs)
  dReal c1,c2,m11,m12,m21,m22;
  c1 = dCalcVectorDot3_14 (center,Ra+code1);
  c2 = dCalcVectorDot3_14 (center,Ra+code2);
  // optimize this? - we have already computed this data above, but it is not
  // stored in an easy-to-index format. for now it's quicker just to recompute
  // the four dot products.
  m11 = dCalcVectorDot3_44 (Ra+code1,Rb+a1);
  m12 = dCalcVectorDot3_44 (Ra+code1,Rb+a2);
  m21 = dCalcVectorDot3_44 (Ra+code2,Rb+a1);
  m22 = dCalcVectorDot3_44 (Ra+code2,Rb+a2);
  {
    dReal k1 = m11*Sb[a1];
    dReal k2 = m21*Sb[a1];
    dReal k3 = m12*Sb[a2];
    dReal k4 = m22*Sb[a2];
    quad[0] = c1 - k1 - k3;
    quad[1] = c2 - k2 - k4;
    quad[2] = c1 - k1 + k3;
    quad[3] = c2 - k2 + k4;
    quad[4] = c1 + k1 + k3;
    quad[5] = c2 + k2 + k4;
    quad[6] = c1 + k1 - k3;
    quad[7] = c2 + k2 - k4;
  }

  // find the size of the reference face
  dReal rect[2];
  rect[0] = Sa[code1];
  rect[1] = Sa[code2];

  // intersect the incident and reference faces
  dReal ret[16];
  int n = intersectRectQuad (rect,quad,ret);
  if (n < 1) return 0;		// this should never happen

  // convert the intersection points into reference-face coordinates,
  // and compute the contact position and depth for each point. only keep
  // those points that have a positive (penetrating) depth. delete points in
  // the 'ret' array as necessary so that 'point' and 'ret' correspond.
  dReal point[3*8];		// penetrating contact points
  dReal dep[8];			// depths for those points
  dReal det1 = dRecip(m11*m22 - m12*m21);
  m11 *= det1;
  m12 *= det1;
  m21 *= det1;
  m22 *= det1;
  int cnum = 0;			// number of penetrating contact points found
  for (j=0; j < n; j++) {
    dReal k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
    dReal k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
    for (i=0; i<3; i++) point[cnum*3+i] =
			  center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
    dep[cnum] = Sa[codeN] - dCalcVectorDot3(normal2,point+cnum*3);
    if (dep[cnum] >= 0) {
      ret[cnum*2] = ret[j*2];
      ret[cnum*2+1] = ret[j*2+1];
      cnum++;
	  if ((cnum | CONTACTS_UNIMPORTANT) == (flags & (NUMC_MASK | CONTACTS_UNIMPORTANT))) {
		  break;
	  }
    }
  }
  if (cnum < 1) { 
	  return 0;	// this should not happen, yet does at times (demo_plane2d single precision).
  }

  // we can't generate more contacts than we actually have
  int maxc = flags & NUMC_MASK;
  if (maxc > cnum) maxc = cnum;
  if (maxc < 1) maxc = 1;	// Even though max count must not be zero this check is kept for backward compatibility as this is a public function

  if (cnum <= maxc) {
    // we have less contacts than we need, so we use them all
    for (j=0; j < cnum; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[j*3+i] + pa[i];
      con->depth = dep[j];
    }
  }
  else {
    dIASSERT(!(flags & CONTACTS_UNIMPORTANT)); // cnum should be generated not greater than maxc so that "then" clause is executed
    // we have more contacts than are wanted, some of them must be culled.
    // find the deepest point, it is always the first contact.
    int i1 = 0;
    dReal maxdepth = dep[0];
    for (i=1; i<cnum; i++) {
      if (dep[i] > maxdepth) {
	maxdepth = dep[i];
	i1 = i;
      }
    }

    int iret[8];
    cullPoints (cnum,ret,maxc,i1,iret);

    for (j=0; j < maxc; j++) {
      dContactGeom *con = CONTACT(contact,skip*j);
      for (i=0; i<3; i++) con->pos[i] = point[iret[j]*3+i] + pa[i];
      con->depth = dep[iret[j]];
    }
    cnum = maxc;
  }

  *return_code = code;
  return cnum;
}
コード例 #19
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x14(void) {		/* 0xED 0x14 : TST  D */
	TST(D);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #20
0
ファイル: ping_common.c プロジェクト: ljegege/ping
int gather_statistics(__u8 *icmph, int icmplen,
		      int cc, __u16 seq, int hops,
		      int csfailed, struct timeval *tv, char *from,
		      void (*pr_reply)(__u8 *icmph, int cc))
{
	int dupflag = 0;
	long triptime = 0;
	__u8 *ptr = icmph + icmplen;

	++nreceived;
	if (!csfailed)
		acknowledge(seq);

	if (timing && cc >= 8+sizeof(struct timeval)) {
		struct timeval tmp_tv;
		memcpy(&tmp_tv, ptr, sizeof(tmp_tv));

restamp:
		tvsub(tv, &tmp_tv);
		triptime = tv->tv_sec * 1000000 + tv->tv_usec;
		if (triptime < 0) {
			fprintf(stderr, "Warning: time of day goes back (%ldus), taking countermeasures.\n", triptime);
			triptime = 0;
			if (!(options & F_LATENCY)) {
				gettimeofday(tv, NULL);
				options |= F_LATENCY;
				goto restamp;
			}
		}
		if (!csfailed) {
			tsum += triptime;
			tsum2 += (long long)triptime * (long long)triptime;
			if (triptime < tmin)
				tmin = triptime;
			if (triptime > tmax)
				tmax = triptime;
			if (!rtt)
				rtt = triptime*8;
			else
				rtt += triptime-rtt/8;
			if (options&F_ADAPTIVE)
				update_interval();
		}
	}

	if (csfailed) {
		++nchecksum;
		--nreceived;
	} else if (TST(seq % mx_dup_ck)) {
		++nrepeats;
		--nreceived;
		dupflag = 1;
	} else {
		SET(seq % mx_dup_ck);
		dupflag = 0;
	}
	confirm = confirm_flag;

	if (options & F_QUIET)
		return 1;

	if (options & F_FLOOD) {
		if (!csfailed)
			write(STDOUT_FILENO, "\b \b", 3);
		else
			write(STDOUT_FILENO, "\bC", 1);
	} else {
		int i;
		__u8 *cp, *dp;

		print_timestamp();
		printf("%d bytes from %s:", cc, from);

		if (pr_reply)
			pr_reply(icmph, cc);

		if (hops >= 0)
			printf(" ttl=%d", hops);

		if (cc < datalen+8) {
			printf(" (truncated)\n");
			return 1;
		}
		if (timing) {
			if (triptime >= 100000)
				printf(" time=%ld ms", triptime/1000);
			else if (triptime >= 10000)
				printf(" time=%ld.%01ld ms", triptime/1000,
				       (triptime%1000)/100);
			else if (triptime >= 1000)
				printf(" time=%ld.%02ld ms", triptime/1000,
				       (triptime%1000)/10);
			else
				printf(" time=%ld.%03ld ms", triptime/1000,
				       triptime%1000);
		}
		if (dupflag)
			printf(" (DUP!)");
		if (csfailed)
			printf(" (BAD CHECKSUM!)");

		/* check the data */
		cp = ((u_char*)ptr) + sizeof(struct timeval);
		dp = &outpack[8 + sizeof(struct timeval)];
		for (i = sizeof(struct timeval); i < datalen; ++i, ++cp, ++dp) {
			if (*cp != *dp) {
				printf("\nwrong data byte #%d should be 0x%x but was 0x%x",
				       i, *dp, *cp);
				cp = (u_char*)ptr + sizeof(struct timeval);
				for (i = sizeof(struct timeval); i < datalen; ++i, ++cp) {
					if ((i % 32) == sizeof(struct timeval))
						printf("\n#%d\t", i);
					printf("%x ", *cp);
				}
				break;
			}
		}
	}
	return 0;
}
コード例 #21
0
void JitArm::lXX(UGeckoInstruction inst)
{
	INSTRUCTION_START
	JITDISABLE(bJITLoadStoreOff);

	u32 a = inst.RA, b = inst.RB, d = inst.RD;
	s32 offset = inst.SIMM_16;
	u32 accessSize = 0;
	s32 offsetReg = -1;
	bool update = false;
	bool signExtend = false;
	bool reverse = false;
	bool fastmem = false;

	switch (inst.OPCD)
	{
		case 31:
			switch (inst.SUBOP10)
			{
				case 55: // lwzux
					update = true;
				case 23: // lwzx
					fastmem = true;
					accessSize = 32;
					offsetReg = b;
				break;
				case 119: //lbzux
					update = true;
				case 87: // lbzx
					fastmem = true;
					accessSize = 8;
					offsetReg = b;
				break;
				case 311: // lhzux
					update = true;
				case 279: // lhzx
					fastmem = true;
					accessSize = 16;
					offsetReg = b;
				break;
				case 375: // lhaux
					update = true;
				case 343: // lhax
					accessSize = 16;
					signExtend = true;
					offsetReg = b;
				break;
				case 534: // lwbrx
					accessSize = 32;
					reverse = true;
				break;
				case 790: // lhbrx
					accessSize = 16;
					reverse = true;
				break;
			}
		break;
		case 33: // lwzu
			update = true;
		case 32: // lwz
			fastmem = true;
			accessSize = 32;
		break;
		case 35: // lbzu
			update = true;
		case 34: // lbz
			fastmem = true;
			accessSize = 8;
		break;
		case 41: // lhzu
			update = true;
		case 40: // lhz
			fastmem = true;
			accessSize = 16;
		break;
		case 43: // lhau
			update = true;
		case 42: // lha
			signExtend = true;
			accessSize = 16;
		break;
	}

	// Check for exception before loading
	ARMReg rA = gpr.GetReg(false);

	LDR(rA, R9, PPCSTATE_OFF(Exceptions));
	TST(rA, EXCEPTION_DSI);
	FixupBranch DoNotLoad = B_CC(CC_NEQ);

	SafeLoadToReg(fastmem, d, update ? a : (a ? a : -1), offsetReg, accessSize, offset, signExtend, reverse);

	if (update)
	{
		ARMReg RA = gpr.R(a);
		if (offsetReg == -1)
		{
			rA = gpr.GetReg(false);
			MOVI2R(rA, offset);
			ADD(RA, RA, rA);
		}
		else
		{
			ADD(RA, RA, gpr.R(offsetReg));
		}
	}

	SetJumpTarget(DoNotLoad);

	// LWZ idle skipping
	if (SConfig::GetInstance().m_LocalCoreStartupParameter.bSkipIdle &&
	    inst.OPCD == 32 &&
	    (inst.hex & 0xFFFF0000) == 0x800D0000 &&
	    (Memory::ReadUnchecked_U32(js.compilerPC + 4) == 0x28000000 ||
	    (SConfig::GetInstance().m_LocalCoreStartupParameter.bWii && Memory::ReadUnchecked_U32(js.compilerPC + 4) == 0x2C000000)) &&
	    Memory::ReadUnchecked_U32(js.compilerPC + 8) == 0x4182fff8)
	{
		ARMReg RD = gpr.R(d);

		// if it's still 0, we can wait until the next event
		TST(RD, RD);
		FixupBranch noIdle = B_CC(CC_NEQ);

		gpr.Flush(FLUSH_MAINTAIN_STATE);
		fpr.Flush(FLUSH_MAINTAIN_STATE);

		rA = gpr.GetReg();

		MOVI2R(rA, (u32)&PowerPC::OnIdle);
		MOVI2R(R0, PowerPC::ppcState.gpr[a] + (s32)(s16)inst.SIMM_16);
		BL(rA);

		gpr.Unlock(rA);
		WriteExceptionExit();

		SetJumpTarget(noIdle);

		//js.compilerPC += 8;
		return;
	}
}
コード例 #22
0
void JitArm::stX(UGeckoInstruction inst)
{
	INSTRUCTION_START
	JITDISABLE(bJITLoadStoreOff);

	u32 a = inst.RA, b = inst.RB, s = inst.RS;
	s32 offset = inst.SIMM_16;
	u32 accessSize = 0;
	s32 regOffset = -1;
	bool update = false;
	bool fastmem = false;
	switch (inst.OPCD)
	{
		case 45: // sthu
			update = true;
		case 44: // sth
			accessSize = 16;
		break;
		case 31:
			switch (inst.SUBOP10)
			{
				case 183: // stwux
					update = true;
				case 151: // stwx
					fastmem = true;
					accessSize = 32;
					regOffset = b;
				break;
				case 247: // stbux
					update = true;
				case 215: // stbx
					accessSize = 8;
					regOffset = b;
				break;
				case 439: // sthux
					update = true;
				case 407: // sthx
					accessSize = 16;
					regOffset = b;
				break;
			}
		break;
		case 37: // stwu
			update = true;
		case 36: // stw
			fastmem = true;
			accessSize = 32;
		break;
		case 39: // stbu
			update = true;
		case 38: // stb
			accessSize = 8;
		break;
	}
	SafeStoreFromReg(fastmem, update ? a : (a ? a : -1), s, regOffset, accessSize, offset);
	if (update)
	{
		ARMReg rA = gpr.GetReg();
		ARMReg RB;
		ARMReg RA = gpr.R(a);
		if (regOffset != -1)
			RB = gpr.R(regOffset);
		// Check for DSI exception prior to writing back address
		LDR(rA, R9, PPCSTATE_OFF(Exceptions));
		TST(rA, EXCEPTION_DSI);
		FixupBranch DoNotWrite = B_CC(CC_NEQ);
		if (a)
		{
			if (regOffset == -1)
			{
				MOVI2R(rA, offset);
				ADD(RA, RA, rA);
			}
			else
			{
				ADD(RA, RA, RB);
			}
		}
		else
		{
			if (regOffset == -1)
				MOVI2R(RA, (u32)offset);
			else
				MOV(RA, RB);
		}
		SetJumpTarget(DoNotWrite);
		gpr.Unlock(rA);
	}
}
コード例 #23
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x34(void) {		/* 0xED 0x34 : TST  (HL) */
	READ_MEM(temp_byte, (HL), 4);
	TST(temp_byte);
	T_WAIT_UNTIL(7);
}
コード例 #24
0
ファイル: ping6.c プロジェクト: nighthawk149/fvs318g-cfw
static void unpack(char *packet, int sz, struct sockaddr_in6 *from, int hoplimit)
{
	struct icmp6_hdr *icmppkt;
	struct timeval tv, *tp;
	int dupflag;
	unsigned long triptime;
	char buf[INET6_ADDRSTRLEN];

	gettimeofday(&tv, NULL);

	/* discard if too short */
	if (sz < (datalen + sizeof(struct icmp6_hdr)))
		return;

	icmppkt = (struct icmp6_hdr *) packet;

	if (icmppkt->icmp6_id != myid)
	    return;				/* not our ping */

	if (icmppkt->icmp6_type == ICMP6_ECHO_REPLY) {
	    ++nreceived;
		tp = (struct timeval *) &icmppkt->icmp6_data8[4];

		if ((tv.tv_usec -= tp->tv_usec) < 0) {
			--tv.tv_sec;
			tv.tv_usec += 1000000;
		}
		tv.tv_sec -= tp->tv_sec;

		triptime = tv.tv_sec * 10000 + (tv.tv_usec / 100);
		tsum += triptime;
		if (triptime < tmin)
			tmin = triptime;
		if (triptime > tmax)
			tmax = triptime;

		if (TST(icmppkt->icmp6_seq % MAX_DUP_CHK)) {
			++nrepeats;
			--nreceived;
			dupflag = 1;
		} else {
			SET(icmppkt->icmp6_seq % MAX_DUP_CHK);
			dupflag = 0;
		}

		if (options & O_QUIET)
			return;

		printf("%d bytes from %s: icmp6_seq=%u", sz,
			   inet_ntop(AF_INET6, (struct in_addr6 *) &pingaddr.sin6_addr,
						 buf, sizeof(buf)),
			   icmppkt->icmp6_seq);
		printf(" ttl=%d time=%lu.%lu ms", hoplimit,
			   triptime / 10, triptime % 10);
		if (dupflag)
			printf(" (DUP!)");
		printf("\n");
	} else
		if (icmppkt->icmp6_type != ICMP6_ECHO_REQUEST)
			bb_error_msg("Warning: Got ICMP %d (%s)",
					icmppkt->icmp6_type, icmp6_type_name (icmppkt->icmp6_type));
}
コード例 #25
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x3C(void) {		/* 0xED 0x3C : TST  A  */
	TST(A);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #26
0
int main(int argc, char *argv[])
{
  int status;
  unsigned char *p;
  int k;

  TSTTITLE("TESTING WRITE MIDIFILE");
  TSTSECTION("Buffer write function");

  TSTGROUP("Buffer sizing");

  status = chk_evt_buf(16);
  TST("Room allocated", (status == 0 && (evt_buf_sz - evt_buf_wm) >= 1024));

  TSTGROUP("Buffer writing");
  p=evt_buf;
  b_write8(0xAB);
  b_write7(0xCD);
  
  #if 0
  b_write16(0xFEDA); /* 1111 1110 1101 1010*/
  b_write14(0xFEDA);
  b_write32(0xFEEDBAC0);
  #endif

  TST("Write  8 bits",(p[0] == 0xAB));
  TST("Write  7 bits",(p[1] == (0xCD & 0x7F)));
  #if 0
  TST("Write 16 bits",(p[2] == 0xFE && p[3] == 0xDA)); /*1111 1110 1101 1010*/
  TST("Write 14 bits",(p[4] == 0x7D && p[5] == 0x5A)); /*0111 1101 0101 1010*/
  TSTONFAIL("Bytes written: %02x %02x",p[4],p[5]);
  #endif

  TSTGROUP("Raw write track");
  h_writetrack = (t_writetrack) mywritetrack1;
  h_error = (t_error) mf_null_handler;

  status = write_track(1);
  TST("write_track() returned unsuccessfully",(status == 309));
  TSTONFAIL("return code: %d\n",status);
  midi_file = fopen("test_file.mid","wb");
  status = write_track(1);
  TST("write_track() returned successfully",(status == 0));
  fclose(midi_file);

  midi_file = fopen("test_file.mid","rb");
  TST("File reopened for read",(midi_file != NULL));

  if (midi_file) {
    /* Now, if everything went well the file should contain:
    *   MTrk 00 00 00 0C 00 90 3C 64 60 3E 64 83 60 FF 2F 00
    */
    TSTWRITE("#");
    p = "MTrk\0\0\0\x0C\0\x90\x3C\x64\x60\x3E\x64\x83\x60\xFF\x2F\0";
    for (k=0; k<20; k++) {
      if (p[k] != fgetc(midi_file)) break;
      TSTWRITE(" %02X",p[k]);
    }
    TSTWRITE("\n");
    TST("File written correctly",(k == 20));

    fclose(midi_file);
    midi_file = NULL;
  }

  h_writetrack = (t_writetrack) mywritetrack2;
  status = mf_write("test_file.mid",0,1,96);
  TST("mf_write succeeded",(status == 0));

  TSTDONE();

  if (midi_file) fclose(midi_file);

  return (0);
}
コード例 #27
0
ファイル: z180ex.c プロジェクト: MEGA65/xemu
static void zop_ED_0x0C(void) {		/* 0xED 0x0C : TST  C */
	TST(C);
	T_WAIT_UNTIL(TST_OP_T_STATES);
}
コード例 #28
0
ファイル: collision.c プロジェクト: mewbak/vu
int dBoxBox2 (const btVector3 p1, const dMatrix3 R1,
		const btVector3 side1, const btVector3 p2,
		const dMatrix3 R2, const btVector3 side2,
		BoxBoxResults *results)
{
	const btScalar fudge_factor = 1.05;
	btVector3 p,pp,normalC;
	normalC[0] = 0.f;
	normalC[1] = 0.f;
	normalC[2] = 0.f;
	const btScalar *normalR = 0;
	btScalar A[3],B[3],R11,R12,R13,R21,R22,R23,R31,R32,R33,
			 Q11,Q12,Q13,Q21,Q22,Q23,Q31,Q32,Q33,s,s2,l,normal[3],depth;
	int i,j,invert_normal,code;

	// get vector from centers of box 1 to box 2, relative to box 1
	p[0] = p2[0] - p1[0];
	p[1] = p2[1] - p1[1];
	p[2] = p2[2] - p1[2];
	dMULTIPLY1_331 (pp,R1,p);		// get pp = p relative to body 1

	// get side lengths (already specified as half lengths)
	A[0] = side1[0];
	A[1] = side1[1];
	A[2] = side1[2];
	B[0] = side2[0];
	B[1] = side2[1];
	B[2] = side2[2];

	// Rij is R1'*R2, i.e. the relative rotation between R1 and R2
	R11 = dDOT44(R1+0,R2+0); R12 = dDOT44(R1+0,R2+1); R13 = dDOT44(R1+0,R2+2);
	R21 = dDOT44(R1+1,R2+0); R22 = dDOT44(R1+1,R2+1); R23 = dDOT44(R1+1,R2+2);
	R31 = dDOT44(R1+2,R2+0); R32 = dDOT44(R1+2,R2+1); R33 = dDOT44(R1+2,R2+2);

	Q11 = btFabs(R11); Q12 = btFabs(R12); Q13 = btFabs(R13);
	Q21 = btFabs(R21); Q22 = btFabs(R22); Q23 = btFabs(R23);
	Q31 = btFabs(R31); Q32 = btFabs(R32); Q33 = btFabs(R33);

	// for all 15 possible separating axes:
	//   * see if the axis separates the boxes. if so, return 0.
	//   * find the depth of the penetration along the separating axis (s2)
	//   * if this is the largest depth so far, record it.
	// the normal vector will be set to the separating axis with the smallest
	// depth. note: normalR is set to point to a column of R1 or R2 if that is
	// the smallest depth normal so far. otherwise normalR is 0 and normalC is
	// set to a vector relative to body 1. invert_normal is 1 if the sign of
	// the normal should be flipped.

#define TST(expr1,expr2,norm,cc) \
	s2 = btFabs(expr1) - (expr2); \
	if (s2 > 0) return 0; \
	if (s2 > s) { \
		s = s2; \
		normalR = norm; \
		invert_normal = ((expr1) < 0); \
		code = (cc); \
	}

	s = -dInfinity;
	invert_normal = 0;
	code = 0;

	// separating axis = u1,u2,u3
	TST (pp[0],(A[0] + B[0]*Q11 + B[1]*Q12 + B[2]*Q13),R1+0,1);
	TST (pp[1],(A[1] + B[0]*Q21 + B[1]*Q22 + B[2]*Q23),R1+1,2);
	TST (pp[2],(A[2] + B[0]*Q31 + B[1]*Q32 + B[2]*Q33),R1+2,3);

	// separating axis = v1,v2,v3
	TST (dDOT41(R2+0,p),(A[0]*Q11 + A[1]*Q21 + A[2]*Q31 + B[0]),R2+0,4);
	TST (dDOT41(R2+1,p),(A[0]*Q12 + A[1]*Q22 + A[2]*Q32 + B[1]),R2+1,5);
	TST (dDOT41(R2+2,p),(A[0]*Q13 + A[1]*Q23 + A[2]*Q33 + B[2]),R2+2,6);

	// note: cross product axes need to be scaled when s is computed.
	// normal (n1,n2,n3) is relative to box 1.
#undef TST
#define TST(expr1,expr2,n1,n2,n3,cc) \
	s2 = btFabs(expr1) - (expr2); \
	if (s2 > SIMD_EPSILON) return 0; \
	l = btSqrt((n1)*(n1) + (n2)*(n2) + (n3)*(n3)); \
	if (l > SIMD_EPSILON) { \
		s2 /= l; \
		if (s2*fudge_factor > s) { \
			s = s2; \
			normalR = 0; \
			normalC[0] = (n1)/l; normalC[1] = (n2)/l; normalC[2] = (n3)/l; \
			invert_normal = ((expr1) < 0); \
			code = (cc); \
		} \
	}

	btScalar fudge2 = 1.0e-5f;

	Q11 += fudge2;
	Q12 += fudge2;
	Q13 += fudge2;

	Q21 += fudge2;
	Q22 += fudge2;
	Q23 += fudge2;

	Q31 += fudge2;
	Q32 += fudge2;
	Q33 += fudge2;

	// separating axis = u1 x (v1,v2,v3)
	TST(pp[2]*R21-pp[1]*R31,(A[1]*Q31+A[2]*Q21+B[1]*Q13+B[2]*Q12),0,-R31,R21,7);
	TST(pp[2]*R22-pp[1]*R32,(A[1]*Q32+A[2]*Q22+B[0]*Q13+B[2]*Q11),0,-R32,R22,8);
	TST(pp[2]*R23-pp[1]*R33,(A[1]*Q33+A[2]*Q23+B[0]*Q12+B[1]*Q11),0,-R33,R23,9);

	// separating axis = u2 x (v1,v2,v3)
	TST(pp[0]*R31-pp[2]*R11,(A[0]*Q31+A[2]*Q11+B[1]*Q23+B[2]*Q22),R31,0,-R11,10);
	TST(pp[0]*R32-pp[2]*R12,(A[0]*Q32+A[2]*Q12+B[0]*Q23+B[2]*Q21),R32,0,-R12,11);
	TST(pp[0]*R33-pp[2]*R13,(A[0]*Q33+A[2]*Q13+B[0]*Q22+B[1]*Q21),R33,0,-R13,12);

	// separating axis = u3 x (v1,v2,v3)
	TST(pp[1]*R11-pp[0]*R21,(A[0]*Q21+A[1]*Q11+B[1]*Q33+B[2]*Q32),-R21,R11,0,13);
	TST(pp[1]*R12-pp[0]*R22,(A[0]*Q22+A[1]*Q12+B[0]*Q33+B[2]*Q31),-R22,R12,0,14);
	TST(pp[1]*R13-pp[0]*R23,(A[0]*Q23+A[1]*Q13+B[0]*Q32+B[1]*Q31),-R23,R13,0,15);

#undef TST

	if (!code) return 0;
	results->code = code;

	// if we get to this point, the boxes interpenetrate. compute the normal
	// in global coordinates.
	if (normalR) {
		normal[0] = normalR[0];
		normal[1] = normalR[4];
		normal[2] = normalR[8];
	}
	else {
		dMULTIPLY0_331 (normal,R1,normalC);
	}
	if (invert_normal) {
		normal[0] = -normal[0];
		normal[1] = -normal[1];
		normal[2] = -normal[2];
	}
	depth = -s;

	// compute contact point(s)

	if (code > 6) {
		// an edge from box 1 touches an edge from box 2.
		// find a point pa on the intersecting edge of box 1
		btVector3 pa;
		btScalar sign;
		for (i=0; i<3; i++) pa[i] = p1[i];
		for (j=0; j<3; j++) {
			sign = (dDOT14(normal,R1+j) > 0) ? 1.0 : -1.0;
			for (i=0; i<3; i++) pa[i] += sign * A[j] * R1[i*4+j];
		}

		// find a point pb on the intersecting edge of box 2
		btVector3 pb;
		for (i=0; i<3; i++) pb[i] = p2[i];
		for (j=0; j<3; j++) {
			sign = (dDOT14(normal,R2+j) > 0) ? -1.0 : 1.0;
			for (i=0; i<3; i++) pb[i] += sign * B[j] * R2[i*4+j];
		}

		btScalar alpha,beta;
		btVector3 ua,ub;
		for (i=0; i<3; i++) ua[i] = R1[((code)-7)/3 + i*4];
		for (i=0; i<3; i++) ub[i] = R2[((code)-7)%3 + i*4];

		dLineClosestApproach (pa,ua,pb,ub,&alpha,&beta);
		for (i=0; i<3; i++) pa[i] += ua[i]*alpha;
		for (i=0; i<3; i++) pb[i] += ub[i]*beta;

		{
			btVector3 pointInWorld;
			addContactPoint(results,normal,pb,depth);
		}
		return 1;
	}

	// okay, we have a face-something intersection (because the separating
	// axis is perpendicular to a face). define face 'a' to be the reference
	// face (i.e. the normal vector is perpendicular to this) and face 'b' to be
	// the incident face (the closest face of the other box).

	const btScalar *Ra,*Rb,*pa,*pb,*Sa,*Sb;
	if (code <= 3) {
		Ra = R1;
		Rb = R2;
		pa = p1;
		pb = p2;
		Sa = A;
		Sb = B;
	}
	else {
		Ra = R2;
		Rb = R1;
		pa = p2;
		pb = p1;
		Sa = B;
		Sb = A;
	}

	// nr = normal vector of reference face dotted with axes of incident box.
	// anr = absolute values of nr.
	btVector3 normal2,nr,anr;
	if (code <= 3) {
		normal2[0] = normal[0];
		normal2[1] = normal[1];
		normal2[2] = normal[2];
	}
	else {
		normal2[0] = -normal[0];
		normal2[1] = -normal[1];
		normal2[2] = -normal[2];
	}
	dMULTIPLY1_331 (nr,Rb,normal2);
	anr[0] = btFabs (nr[0]);
	anr[1] = btFabs (nr[1]);
	anr[2] = btFabs (nr[2]);

	// find the largest compontent of anr: this corresponds to the normal
	// for the indident face. the other axis numbers of the indicent face
	// are stored in a1,a2.
	int lanr,a1,a2;
	if (anr[1] > anr[0]) {
		if (anr[1] > anr[2]) {
			a1 = 0;
			lanr = 1;
			a2 = 2;
		}
		else {
			a1 = 0;
			a2 = 1;
			lanr = 2;
		}
	}
	else {
		if (anr[0] > anr[2]) {
			lanr = 0;
			a1 = 1;
			a2 = 2;
		}
		else {
			a1 = 0;
			a2 = 1;
			lanr = 2;
		}
	}

	// compute center point of incident face, in reference-face coordinates
	btVector3 center;
	if (nr[lanr] < 0) {
		for (i=0; i<3; i++) center[i] = pb[i] - pa[i] + Sb[lanr] * Rb[i*4+lanr];
	}
	else {
		for (i=0; i<3; i++) center[i] = pb[i] - pa[i] - Sb[lanr] * Rb[i*4+lanr];
	}

	// find the normal and non-normal axis numbers of the reference box
	int codeN,code1,code2;
	if (code <= 3) codeN = code-1; else codeN = code-4;
	if (codeN==0) {
		code1 = 1;
		code2 = 2;
	}
	else if (codeN==1) {
		code1 = 0;
		code2 = 2;
	}
	else {
		code1 = 0;
		code2 = 1;
	}

	// find the four corners of the incident face, in reference-face coordinates
	btScalar quad[8];	// 2D coordinate of incident face (x,y pairs)
	btScalar c1,c2,m11,m12,m21,m22;
	c1 = dDOT14 (center,Ra+code1);
	c2 = dDOT14 (center,Ra+code2);
	// optimize this? - we have already computed this data above, but it is not
	// stored in an easy-to-index format. for now it's quicker just to recompute
	// the four dot products.
	m11 = dDOT44 (Ra+code1,Rb+a1);
	m12 = dDOT44 (Ra+code1,Rb+a2);
	m21 = dDOT44 (Ra+code2,Rb+a1);
	m22 = dDOT44 (Ra+code2,Rb+a2);
	{
		btScalar k1 = m11*Sb[a1];
		btScalar k2 = m21*Sb[a1];
		btScalar k3 = m12*Sb[a2];
		btScalar k4 = m22*Sb[a2];
		quad[0] = c1 - k1 - k3;
		quad[1] = c2 - k2 - k4;
		quad[2] = c1 - k1 + k3;
		quad[3] = c2 - k2 + k4;
		quad[4] = c1 + k1 + k3;
		quad[5] = c2 + k2 + k4;
		quad[6] = c1 + k1 - k3;
		quad[7] = c2 + k2 - k4;
	}

	// find the size of the reference face
	btScalar rect[2];
	rect[0] = Sa[code1];
	rect[1] = Sa[code2];

	// intersect the incident and reference faces
	btScalar ret[16];
	int n = intersectRectQuad2 (rect,quad,ret);
	if (n < 1) return 0;		// this should never happen

	// convert the intersection points into reference-face coordinates,
	// and compute the contact position and depth for each point. only keep
	// those points that have a positive (penetrating) depth. delete points in
	// the 'ret' array as necessary so that 'point' and 'ret' correspond.
	btScalar point[3*8];		// penetrating contact points
	btScalar dep[8];			// depths for those points
	btScalar det1 = 1.f/(m11*m22 - m12*m21);
	m11 *= det1;
	m12 *= det1;
	m21 *= det1;
	m22 *= det1;
	int cnum = 0;			// number of penetrating contact points found
	for (j=0; j < n; j++) {
		btScalar k1 =  m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
		btScalar k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
		for (i=0; i<3; i++) point[cnum*3+i] =
			center[i] + k1*Rb[i*4+a1] + k2*Rb[i*4+a2];
		dep[cnum] = Sa[codeN] - dDOT(normal2,point+cnum*3);
		if (dep[cnum] >= 0) {
			ret[cnum*2] = ret[j*2];
			ret[cnum*2+1] = ret[j*2+1];
			cnum++;
		}
	}
	if (cnum < 1) return 0;	// this should never happen

	// we can't generate more contacts than we actually have
	int maxc = 4;
	if (maxc > cnum) maxc = cnum;
	if (maxc < 1) maxc = 1;

	if (cnum <= maxc)
	{
		if (code<4)
		{
			// we have less contacts than we need, so we use them all
			for (j=0; j < cnum; j++)
			{
				btVector3 pointInWorld;
				for (i=0; i<3; i++)
					pointInWorld[i] = point[j*3+i] + pa[i];
				addContactPoint(results,normal,pointInWorld,dep[j]);
			}
		}
		else
		{
			// we have less contacts than we need, so we use them all
			for (j=0; j < cnum; j++)
			{
				btVector3 pointInWorld;
				for (i=0; i<3; i++)
					pointInWorld[i] = point[j*3+i] + pa[i]-normal[i]*dep[j];
				addContactPoint(results,normal,pointInWorld,dep[j]);
			}
		}
	}
	else {
		// we have more contacts than are wanted, some of them must be culled.
		// find the deepest point, it is always the first contact.
		int i1 = 0;
		btScalar maxdepth = dep[0];
		for (i=1; i<cnum; i++)
		{
			if (dep[i] > maxdepth)
			{
				maxdepth = dep[i];
				i1 = i;
			}
		}

		int iret[8];
		cullPoints2 (cnum,ret,maxc,i1,iret);

		for (j=0; j < maxc; j++)
		{
			btVector3 posInWorld;
			for (i=0; i<3; i++)
				posInWorld[i] = point[iret[j]*3+i] + pa[i];
			if (code<4)
			{
				addContactPoint(results, normal,posInWorld,dep[iret[j]]);
			}
			else
			{
				posInWorld[0] -= normal[0]*dep[iret[j]];
				posInWorld[1] -= normal[1]*dep[iret[j]];
				posInWorld[2] -= normal[2]*dep[iret[j]];
				addContactPoint(results, normal,posInWorld,dep[iret[j]]);
			}
		}
		cnum = maxc;
	}
	return cnum;
}
コード例 #29
0
ファイル: test.c プロジェクト: andresbmth/Trabajo_1_Micros
int main()
{
	int op;
	uint32_t registro[16]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
	char banderas[4];
	do{
		system("cls");
		printf("seleccione la opcion 1 para mostrar los valores de los registros\n");
		printf("seleccione la opcion 2 para sumar registros \n");
		printf("seleccione la opcion 3 para multiplicacion logica (AND) de registros \n");
		printf("seleccione la opcion 4 para Eor a nivel de bits \n");
		printf("seleccione la opcion 5 para desplazar de un registro a otro \n");
		printf("seleccione la opcion 6 para suma logica (OR) de registro\n");
		printf("seleccione la opcion 7 para ADN sin almacenar, solo modifica banderas \n");
		printf("seleccione la opcion 8 para comparar (SUB sin almacenar), solo modifica banderas\n");
		printf("seleccione la opcion 9 Multiplicacion de registros, solo se alacenan 32 bits menos significativos\n");
		printf("seleccione la opcion 10 AND sin almacenacmiento, solo modifica banderas\n");
		printf("seleccione la opcion 11 para  LSL desplazamiento logico a la izquierda \n");
		printf("seleccione la opcion 12 para  LSR desplazamiento logico a la derecha \n");
		printf("seleccione la opcion 13 para  ROR rotacion a la derecha \n");
		printf("seleccione la opcion 14 para  ASR desplazamiento aritmetico a la derecha \n");
		printf("seleccione la opcion 15 para  BIC Realiza una AND de un registro con otro negado \n");
		printf("seleccione la opcion 16 para  MUN guarda en un registro la negacion de otro\n");
		printf("seleccione la opcion 17 para  RSB niega un valor de registro\n");
		printf("seleccione la opcion 18 para  NOP da un retardo de un ciclo de reloj (no hace nada) \n");
		printf("seleccione la opcion 19 para  REV toma grupos de 8 bits y los desplaza \n");
		printf("seleccione la opcion 20 para  REVIG toma grupos de 16 bits y los agrupa en grupos de dos bytes\n");
		printf("seleccione la opcion 21 para  REVSH extencion con signo\n\n");
		
		scanf("%d",&op);
		
		system("cls");
		switch(op){		

			case 1:			
				//mostrar_valores(registro);			
			break;
			
			case 2:			
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
				
				ADD(registro,&registro[0],registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 3:				
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
					
				AND(registro,&registro[0],registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 4:
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
				
				EOR(registro,&registro[0],registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 5:
				printf("ingrese el valor del registro origen:\n");
				scanf("%d",&registro[1]);
				
				MOV(registro,&registro[0],registro[1],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 7:
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
				
				CMN(registro,registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 8:
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
				
				CMP(registro,registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 9:			
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
				
				MUL(registro,&registro[0],registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 10:			
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[2]);
				
				TST(registro,registro[1],registro[2],&banderas[0]);
				
				printf("%d valor del resultado \n",registro[0]);
				printf("%d valor del resultado bandera n \n",banderas[N]);
				printf("%d valor del resultado bandera z \n",banderas[Z]);
				printf("%d valor del resultado bandera c \n",banderas[C]);
				printf("%d valor del resultado bandera v \n",banderas[V]);
			break;
			
			case 11:			
				printf("ingrese el valor del registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese el numero de desplazamientos:\n");
				scanf("%d",&registro[2]);
				
				LSL(registro,&registro[0],registro[1],registro[2],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 12:
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese elnumero de desplazamientos:\n");
				scanf("%d",&registro[2]);
				
				LSR(registro,&registro[0],registro[1],registro[2],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;			
			
			case 13:			
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese elnumero de desplazamientos:\n");
				scanf("%d",&registro[2]);
				
				ROR(registro,&registro[0],registro[1],registro[2],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 14:			
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[1]);
				printf("ingrese elnumero de desplazamientos:\n");
				scanf("%d",&registro[2]);
				
				ASR(registro,&registro[0],registro[1],registro[2],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;

			case 15:			
				printf("ingrese el valor del primer registro:\n");
				scanf("%d",&registro[0]);
				printf("ingrese el valor del segundo registro:\n");
				scanf("%d",&registro[1]);
				
				BIC(registro,&registro[0],registro[1],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 16:			
				printf("ingrese un valor del registro origen\n");
				scanf("%d",&registro[1]);
				
				MVN(registro,&registro[0],registro[1],banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 17:			
				printf("ingrese un valor de registro\n");
				scanf("%d",&registro[1]);	
				
				RSB(registro,&registro[0],registro[1],0,banderas);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 18:			
				NOP(registro);
			break;
			
			case 19:			
				printf("ingrese un valor de registro \n");
				scanf("%d",&registro[0]);
				
				REV(registro,&registro[0]);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 20:			
				printf("ingrese un valor de registro \n");
				scanf("%d",&registro[0]);
				
				REVIG(registro,&registro[0]);
				
				printf("%d valor del resultado \n",registro[0]);
			break;
			
			case 21:			
				printf("ingrese un valor de registro \n");
				scanf("%d",&registro[0]);
				
				REVSH(registro,&registro[0]);
				
				printf("%d valor del resultado \n",registro[0]);
			break;	
			
			default:
				printf("Opcion invalida\n\n");
			break;
		}
		printf("\nDesea realizar otra operacion?\n<1>-si\n<0>-no\n");
		scanf("%d",&op);
		system("cls");
		}while(op);
		return 0;
}
コード例 #30
0
ファイル: main.cpp プロジェクト: Moondee/Artemis
int main(int argc, char ** argv) {
    memory::initialize(0);
    bool do_display_usage = false;
    parse_cmd_line_args(argc, argv, do_display_usage);
    TST_ARGV(grobner);
    TST(random);
    TST(vector);
    TST(symbol_table);
    TST(region);
    TST(symbol);
    TST(heap);
    TST(hashtable);
    TST_ARGV(smtparser);
    TST(rational);
    TST(inf_rational);
    TST(ast);
    TST(optional);
    TST(bit_vector);
    TST(ast_pp);
    TST(ast_smt_pp);
    TST_ARGV(expr_delta);
    TST(string_buffer);
    TST(map);
    TST(diff_logic);
    TST(uint_set);
    TST_ARGV(expr_rand);
    TST(expr_context_simplifier);
    TST(ini_file);
    TST(expr_pattern_match);
    TST(list);
    TST(small_object_allocator);
    TST(timeout);
    TST(splay_tree);
    TST(fvi);
    TST(proof_checker);
    TST(simplifier);
    TST(bv_simplifier_plugin);
    TST(bit_blaster);
    TST(var_subst);
    TST(simple_parser);
    TST(symmetry);
    TST_ARGV(symmetry_parse);
    TST_ARGV(symmetry_prove);
    TST(api);
    TST(old_interval);
    TST(interval_skip_list);
    TST(no_overflow);
    TST(memory);
    TST(parallel);
    TST(get_implied_equalities);
    TST(arith_simplifier_plugin);
    TST(quant_elim);
    TST(matcher);
    TST(datalog_parser);
    TST(dl_rule_set);
    TST_ARGV(datalog_parser_file);
    TST(object_allocator);
    TST(mpz);
    TST(mpq);
    TST(mpf);
    TST(total_order);
    TST(dl_table);
    TST(dl_context);
    TST(dl_smt_relation);
    TST(dl_query);
    TST(dl_util);
    TST(dl_product_relation);
    TST(dl_relation);
    TST(imdd);
    TST(array_property_expander);
    TST(parray);
    TST(stack);
    TST(escaped);
    TST(buffer);
    TST(chashtable);
    TST(ex);
    TST(nlarith_util);
    TST(api_bug);
    TST(arith_rewriter);
    TST(check_assumptions);
    TST(smt_context);
    TST(theory_dl);
    TST(model_retrieval);
    TST(factor_rewriter);
    TST(smt2print_parse);
    TST(substitution);
    TST(polynomial);
    TST(upolynomial);
    TST(algebraic);
    TST(polynomial_factorization);
    TST(prime_generator);
    TST(permutation);
    TST(nlsat);
    TST(qe_defs);
    TST(ext_numeral);
    TST(interval);
    TST(quant_solve);
    TST(f2n);
    TST(hwf);
    TST(trigo);
    TST(bits);
    TST(mpbq);
    TST(mpfx);
    TST(mpff);
    TST(horn_subsume_model_converter);
    TST(model2expr);
}