コード例 #1
0
void Object::setRotation(float x,float y,float z)
{
	dMatrix3 R,R0,R1,R2,R3;

	dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(x));
	dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(y));
	dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(z));
	dMultiply0 (R0,R1,R2,3,3,3);
	dMultiply0 (R, R0,R3,3,3,3);

	dGeomSetRotation(iGeom,R);
	dBodySetRotation(iBody,R);
}
コード例 #2
0
void Object::MakeGeom(dSpaceID space)
{
	iGeom = dCreateBox(space,iSize.x,iSize.y,iSize.z);

	dMatrix3 R,R0,R1,R2,R3;

	dRFromAxisAndAngle(R1,1,0,0,DEG2RAD(iRotate.x));
	dRFromAxisAndAngle(R2,0,1,0,DEG2RAD(iRotate.y));
	dRFromAxisAndAngle(R3,0,0,1,DEG2RAD(iRotate.z));
	dMultiply0 (R0,R1,R2,3,3,3);
	dMultiply0 (R, R0,R3,3,3,3);

	dGeomSetRotation(iGeom,R);
	dGeomSetPosition(iGeom, iPosition.x, iPosition.y, iPosition.z);
};
コード例 #3
0
ファイル: demo_ode.cpp プロジェクト: Devilmore/GoalBabbling
void testInvertPDMatrix()
{
  int i,j,ok;
  dReal A[MSIZE4*MSIZE], Ainv[MSIZE4*MSIZE], I[MSIZE4*MSIZE];
  HEADER;

  dMakeRandomMatrix (A,MSIZE,MSIZE,1.0);
  dMultiply2 (Ainv,A,A,MSIZE,MSIZE,MSIZE);
  memcpy (A,Ainv,MSIZE4*MSIZE*sizeof(dReal));
  dSetZero (Ainv,MSIZE4*MSIZE);

  if (dInvertPDMatrix (A,Ainv,MSIZE))
    printf ("\tpassed (1)\n"); else printf ("\tFAILED (1)\n");
  dMultiply0 (I,A,Ainv,MSIZE,MSIZE,MSIZE);

  // compare with identity
  ok = 1;
  for (i=0; i<MSIZE; i++) {
    for (j=0; j<MSIZE; j++) {
      if (i != j) if (cmp (I[i*MSIZE4+j],0.0)==0) ok = 0;
    }
  }
  for (i=0; i<MSIZE; i++) {
    if (cmp (I[i*MSIZE4+i],1.0)==0) ok = 0;
  }
  if (ok) printf ("\tpassed (2)\n"); else printf ("\tFAILED (2)\n");
}
コード例 #4
0
ファイル: demo_ode.cpp プロジェクト: Devilmore/GoalBabbling
void testMatrixMultiply()
{
  // A is 2x3, B is 3x4, B2 is B except stored columnwise, C is 2x4
  dReal A[8],B[12],A2[12],B2[16],C[8];
  int i;

  HEADER;
  dSetZero (A,8);
  for (i=0; i<3; i++) A[i] = i+2;
  for (i=0; i<3; i++) A[i+4] = i+3+2;
  for (i=0; i<12; i++) B[i] = i+8;
  dSetZero (A2,12);
  for (i=0; i<6; i++) A2[i+2*(i/2)] = A[i+i/3];
  dSetZero (B2,16);
  for (i=0; i<12; i++) B2[i+i/3] = B[i];

  dMultiply0 (C,A,B,2,3,4);
  if (C[0] != 116 || C[1] != 125 || C[2] != 134 || C[3] != 143 ||
      C[4] != 224 || C[5] != 242 || C[6] != 260 || C[7] != 278)
    printf ("\tFAILED (1)\n"); else printf ("\tpassed (1)\n");

  dMultiply1 (C,A2,B,2,3,4);
  if (C[0] != 160 || C[1] != 172 || C[2] != 184 || C[3] != 196 ||
      C[4] != 196 || C[5] != 211 || C[6] != 226 || C[7] != 241)
    printf ("\tFAILED (2)\n"); else printf ("\tpassed (2)\n");

  dMultiply2 (C,A,B2,2,3,4);
  if (C[0] != 83 || C[1] != 110 || C[2] != 137 || C[3] != 164 ||
      C[4] != 164 || C[5] != 218 || C[6] != 272 || C[7] != 326)
    printf ("\tFAILED (3)\n"); else printf ("\tpassed (3)\n");
}
コード例 #5
0
ファイル: sslworld.cpp プロジェクト: Cyberhornet/grSim
bool wheelCallBack(dGeomID o1,dGeomID o2,PSurface* s)
{
    //s->id2 is ground
    const dReal* r; //wheels rotation matrix
    //const dReal* p; //wheels rotation matrix
    if ((o1==s->id1) && (o2==s->id2)) {
        r=dBodyGetRotation(dGeomGetBody(o1));
        //p=dGeomGetPosition(o1);//never read
    } else if ((o1==s->id2) && (o2==s->id1)) {
        r=dBodyGetRotation(dGeomGetBody(o2));
        //p=dGeomGetPosition(o2);//never read
    } else {
        //XXX: in this case we dont have the rotation
        //     matrix, thus we must return
        return false;
    }

    s->surface.mode = dContactFDir1 | dContactMu2  | dContactApprox1 | dContactSoftCFM;
    s->surface.mu = fric(_w->cfg->robotSettings.WheelPerpendicularFriction);
    s->surface.mu2 = fric(_w->cfg->robotSettings.WheelTangentFriction);
    s->surface.soft_cfm = 0.002;

    dVector3 v={0,0,1,1};
    dVector3 axis;
    dMultiply0(axis,r,v,4,3,1);
    dReal l = sqrt(axis[0]*axis[0] + axis[1]*axis[1]);
    s->fdir1[0] = axis[0]/l;
    s->fdir1[1] = axis[1]/l;
    s->fdir1[2] = 0;
    s->fdir1[3] = 0;
    s->usefdir1 = true;
    return true;
}
コード例 #6
0
ファイル: demo_ode.cpp プロジェクト: Devilmore/GoalBabbling
void testSmallMatrixMultiply()
{
  dMatrix3 A,B,C,A2;
  dVector3 a,a2,x;

  HEADER;
  dMakeRandomMatrix (A,3,3,1.0);
  dMakeRandomMatrix (B,3,3,1.0);
  dMakeRandomMatrix (C,3,3,1.0);
  dMakeRandomMatrix (x,3,1,1.0);

  // dMultiply0_331()
  dMultiply0_331 (a,B,x);
  dMultiply0 (a2,B,x,3,3,1);
  printf ("\t%s (1)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
	  "passed");

  // dMultiply1_331()
  dMultiply1_331 (a,B,x);
  dMultiply1 (a2,B,x,3,3,1);
  printf ("\t%s (2)\n",(dMaxDifference (a,a2,3,1) > tol) ? "FAILED" :
	  "passed");

  // dMultiply0_133
  dMultiply0_133 (a,x,B);
  dMultiply0 (a2,x,B,1,3,3);
  printf ("\t%s (3)\n",(dMaxDifference (a,a2,1,3) > tol) ? "FAILED" :
	  "passed");

  // dMultiply0_333()
  dMultiply0_333 (A,B,C);
  dMultiply0 (A2,B,C,3,3,3);
  printf ("\t%s (4)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
	  "passed");

  // dMultiply1_333()
  dMultiply1_333 (A,B,C);
  dMultiply1 (A2,B,C,3,3,3);
  printf ("\t%s (5)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
	  "passed");

  // dMultiply2_333()
  dMultiply2_333 (A,B,C);
  dMultiply2 (A2,B,C,3,3,3);
  printf ("\t%s (6)\n",(dMaxDifference (A,A2,3,3) > tol) ? "FAILED" :
	  "passed");
}
コード例 #7
0
static void simLoop (int pause)
{
  // stop after a given number of iterations, as long as we are not in
  // interactive mode
  if (cmd_graphics && !cmd_interactive &&
      (iteration >= max_iterations)) {
    dsStop();
    return;
  }
  iteration++;

  if (!pause) {
    // do stuff for this test and check to see if the joint is behaving well
    dReal error = doStuffAndGetError (test_num);
    if (error > max_error) max_error = error;
    if (cmd_interactive && error < dInfinity) {
      printf ("scaled error = %.4e\n",error);
    }

    // take a step
    dWorldStep (world,STEPSIZE);

    // occasionally re-orient the first body to create a deliberate error.
    if (cmd_occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[0]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[0],R3);

	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[0]);
	dBodySetPosition (body[0],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }
  }

  if (cmd_graphics) {
    dReal sides1[3] = {SIDE,SIDE,SIDE};
    dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
    dsSetTexture (DS_WOOD);
    dsSetColor (1,1,0);
    dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
    if (body[1]) {
      dsSetColor (0,1,1);
      dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
    }
  }
}
コード例 #8
0
static void simLoop (int pause)
{
  const dReal kd = -0.3;	// angular damping constant
  const dReal ks = 0.5;	// spring constant
  if (!pause) {
    // add an oscillating torque to body 0, and also damp its rotational motion
    static dReal a=0;
    const dReal *w = dBodyGetAngularVel (body[0]);
    dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
    a += 0.01;

    // add a spring force to keep the bodies together, otherwise they will
    // fly apart along the slider axis.
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
		   ks*(p2[2]-p1[2]));
    dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
		   ks*(p1[2]-p2[2]));

    // occasionally re-orient one of the bodies to create a deliberate error.
    if (occasional_error) {
      static int count = 0;
      if ((count % 20)==0) {
	// randomly adjust orientation of body[0]
	const dReal *R1;
	dMatrix3 R2,R3;
	R1 = dBodyGetRotation (body[0]);
	dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
			    dRandReal()-0.5,dRandReal()-0.5);
	dMultiply0 (R3,R1,R2,3,3,3);
	dBodySetRotation (body[0],R3);

	// randomly adjust position of body[0]
	const dReal *pos = dBodyGetPosition (body[0]);
	dBodySetPosition (body[0],
			  pos[0]+0.2*(dRandReal()-0.5),
			  pos[1]+0.2*(dRandReal()-0.5),
			  pos[2]+0.2*(dRandReal()-0.5));
      }
      count++;
    }

    dWorldStep (world,0.05);
  }

  dReal sides1[3] = {SIDE,SIDE,SIDE};
  dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
  dsSetTexture (DS_WOOD);
  dsSetColor (1,1,0);
  dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
  dsSetColor (0,1,1);
  dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
コード例 #9
0
ファイル: demo_ode.cpp プロジェクト: Devilmore/GoalBabbling
void testQuaternionMultiply()
{
  HEADER;
  dMatrix3 RA,RB,RC,Rtest;
  dQuaternion qa,qb,qc;
  dReal diff,maxdiff=0;

  for (int i=0; i<100; i++) {
    makeRandomRotation (RB);
    makeRandomRotation (RC);
    dRtoQ (RB,qb);
    dRtoQ (RC,qc);

    dMultiply0 (RA,RB,RC,3,3,3);
    dQMultiply0 (qa,qb,qc);
    dQtoR (qa,Rtest);
    diff = dMaxDifference (Rtest,RA,3,3);
    if (diff > maxdiff) maxdiff = diff;

    dMultiply1 (RA,RB,RC,3,3,3);
    dQMultiply1 (qa,qb,qc);
    dQtoR (qa,Rtest);
    diff = dMaxDifference (Rtest,RA,3,3);
    if (diff > maxdiff) maxdiff = diff;

    dMultiply2 (RA,RB,RC,3,3,3);
    dQMultiply2 (qa,qb,qc);
    dQtoR (qa,Rtest);
    diff = dMaxDifference (Rtest,RA,3,3);
    if (diff > maxdiff) maxdiff = diff;

    dMultiply0 (RA,RC,RB,3,3,3);
    transpose3x3 (RA);
    dQMultiply3 (qa,qb,qc);
    dQtoR (qa,Rtest);
    diff = dMaxDifference (Rtest,RA,3,3);
    if (diff > maxdiff) maxdiff = diff;
  }
  printf ("\tmaximum difference = %e - %s\n",maxdiff,
	  (maxdiff > tol) ? "FAILED" : "passed");
}
コード例 #10
0
ファイル: robot.cpp プロジェクト: KRSSG/robocupssl_old
void Robot::setDir(float ang)
{
    ang*=M_PI/180.0f;
    chassis->setBodyRotation(0,0,1,ang);
    kicker->box->setBodyRotation(0,0,1,ang);
    dummy->setBodyRotation(0,0,1,ang);
    dMatrix3 wLocalRot,wRot,cRot;
    dVector3 localPos,finalPos,cPos;
    chassis->getBodyPosition(cPos[0],cPos[1],cPos[2],false);
    chassis->getBodyRotation(cRot,false);
    kicker->box->getBodyPosition(localPos[0],localPos[1],localPos[2],true);
    dMultiply0(finalPos,cRot,localPos,4,3,1);finalPos[0]+=cPos[0];finalPos[1]+=cPos[1];finalPos[2]+=cPos[2];
    kicker->box->setBodyPosition(finalPos[0],finalPos[1],finalPos[2],false);
    for (int i=0;i<4;i++)
    {
        wheels[i]->cyl->getBodyRotation(wLocalRot,true);
        dMultiply0(wRot,cRot,wLocalRot,3,3,3);
        dBodySetRotation(wheels[i]->cyl->body,wRot);
        wheels[i]->cyl->getBodyPosition(localPos[0],localPos[1],localPos[2],true);
        dMultiply0(finalPos,cRot,localPos,4,3,1);finalPos[0]+=cPos[0];finalPos[1]+=cPos[1];finalPos[2]+=cPos[2];
        wheels[i]->cyl->setBodyPosition(finalPos[0],finalPos[1],finalPos[2],false);
    }
}
コード例 #11
0
ファイル: demo_ode.cpp プロジェクト: Devilmore/GoalBabbling
void testCrossProduct()
{
  HEADER;

  dVector3 a1,a2,b,c;
  dMatrix3 B;
  dMakeRandomVector (b,3,1.0);
  dMakeRandomVector (c,3,1.0);

  dCalcVectorCross3(a1,b,c);

  dSetZero (B,12);
  dSetCrossMatrixPlus(B,b,4);
  dMultiply0 (a2,B,c,3,3,1);

  dReal diff = dMaxDifference(a1,a2,3,1);
  printf ("\t%s\n", diff > tol ? "FAILED" : "passed");
}
コード例 #12
0
ファイル: lcp.cpp プロジェクト: dartsim/dart
extern "C" ODE_API int dTestSolveLCP()
{
  const int n = 100;

  //size_t memreq = EstimateTestSolveLCPMemoryReq(n);
  //dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, nullptr, nullptr);
  //if (arena == nullptr) {
  //  return 0;
  //}

  //int i,nskip = dPAD(n);
  int i;
  int nskip = n;
#ifdef dDOUBLE
  const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
  const dReal tol = REAL(1e-4);
#endif
  printf ("dTestSolveLCP()\n");

  dReal *A = new dReal[n*nskip];
  dReal *x = new dReal[n];
  dReal *b = new dReal[n];
  dReal *w = new dReal[n];
  dReal *lo = new dReal[n];
  dReal *hi = new dReal[n];
  
  dReal *A2 = new dReal[n*nskip];
  dReal *b2 = new dReal[n];
  dReal *lo2 = new dReal[n];
  dReal *hi2 = new dReal[n];
  
  dReal *tmp1 = new dReal[n];
  dReal *tmp2 = new dReal[n];

  // double total_time = 0;
  for (int count=0; count < 1000; count++) {

      // form (A,b) = a random positive definite LCP problem
      dMakeRandomMatrix (A2,n,n,1.0);
      dMultiply2 (A,A2,A2,n,n,n);
      dMakeRandomMatrix (x,n,1,1.0);
      dMultiply0 (b,A,x,n,n,1);
      for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);

      // choose `nub' in the range 0..n-1
      int nub = 50; //dRandInt (n);

      // make limits
      for (i=0; i<nub; i++) lo[i] = -dInfinity;
      for (i=0; i<nub; i++) hi[i] = dInfinity;
      //for (i=nub; i<n; i++) lo[i] = 0;
      //for (i=nub; i<n; i++) hi[i] = dInfinity;
      //for (i=nub; i<n; i++) lo[i] = -dInfinity;
      //for (i=nub; i<n; i++) hi[i] = 0;
      for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
      for (i=nub; i<n; i++) hi[i] =  (dRandReal()*REAL(1.0))+REAL(0.01);

      // set a few limits to lo=hi=0
      /*
      for (i=0; i<10; i++) {
      int j = dRandInt (n-nub) + nub;
      lo[j] = 0;
      hi[j] = 0;
      }
      */

      // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
      // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
      // ensure that it doesn't get referenced (if it does, the answer will be
      // wrong).

      memcpy (A2,A,n*nskip*sizeof(dReal));
      dClearUpperTriangle (A2,n);
      memcpy (b2,b,n*sizeof(dReal));
      memcpy (lo2,lo,n*sizeof(dReal));
      memcpy (hi2,hi,n*sizeof(dReal));
      dSetZero (x,n);
      dSetZero (w,n);

 
      dSolveLCP (n,A2,x,b2,w,nub,lo2,hi2,0);

      // check the solution

      dMultiply0 (tmp1,A,x,n,n,1);
      for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
      dReal diff = dMaxDifference (tmp1,tmp2,n,1);
      // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
      //	    diff > tol ? "FAILED" : "passed");
      if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
      int n1=0,n2=0,n3=0;
      for (i=0; i<n; i++) {
        if (x[i]==lo[i] && w[i] >= 0) {
          n1++;	// ok
        }
        else if (x[i]==hi[i] && w[i] <= 0) {
          n2++;	// ok
        }
        else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
          n3++;	// ok
        }
        else {
          dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
            x[i],w[i],lo[i],hi[i]);
        }
      }

      // pacifier
      printf ("passed: NL=%3d NH=%3d C=%3d   ",n1,n2,n3);
  }
 
  delete[] A;
  delete[] x;
  delete[] b;
  delete[] w;
  delete[] lo ;
  delete[] hi ;

  delete[] A2 ;
  delete[] b2 ;
  delete[] lo2;
  delete[] hi2;
  
  delete[] tmp1;
  delete[] tmp2;

  return 1;
}
コード例 #13
0
ファイル: lcp.cpp プロジェクト: Devilmore/GoalBabbling
extern "C" ODE_API int dTestSolveLCP()
{
  const int n = 100;

  size_t memreq = EstimateTestSolveLCPMemoryReq(n);
  dxWorldProcessMemArena *arena = dxAllocateTemporaryWorldProcessMemArena(memreq, NULL, NULL);
  if (arena == NULL) {
    return 0;
  }

  int i,nskip = dPAD(n);
#ifdef dDOUBLE
  const dReal tol = REAL(1e-9);
#endif
#ifdef dSINGLE
  const dReal tol = REAL(1e-4);
#endif
  printf ("dTestSolveLCP()\n");

  dReal *A = arena->AllocateArray<dReal> (n*nskip);
  dReal *x = arena->AllocateArray<dReal> (n);
  dReal *b = arena->AllocateArray<dReal> (n);
  dReal *w = arena->AllocateArray<dReal> (n);
  dReal *lo = arena->AllocateArray<dReal> (n);
  dReal *hi = arena->AllocateArray<dReal> (n);
  
  dReal *A2 = arena->AllocateArray<dReal> (n*nskip);
  dReal *b2 = arena->AllocateArray<dReal> (n);
  dReal *lo2 = arena->AllocateArray<dReal> (n);
  dReal *hi2 = arena->AllocateArray<dReal> (n);
  
  dReal *tmp1 = arena->AllocateArray<dReal> (n);
  dReal *tmp2 = arena->AllocateArray<dReal> (n);

  double total_time = 0;
  for (int count=0; count < 1000; count++) {
    BEGIN_STATE_SAVE(arena, saveInner) {

      // form (A,b) = a random positive definite LCP problem
      dMakeRandomMatrix (A2,n,n,1.0);
      dMultiply2 (A,A2,A2,n,n,n);
      dMakeRandomMatrix (x,n,1,1.0);
      dMultiply0 (b,A,x,n,n,1);
      for (i=0; i<n; i++) b[i] += (dRandReal()*REAL(0.2))-REAL(0.1);

      // choose `nub' in the range 0..n-1
      int nub = 50; //dRandInt (n);

      // make limits
      for (i=0; i<nub; i++) lo[i] = -dInfinity;
      for (i=0; i<nub; i++) hi[i] = dInfinity;
      //for (i=nub; i<n; i++) lo[i] = 0;
      //for (i=nub; i<n; i++) hi[i] = dInfinity;
      //for (i=nub; i<n; i++) lo[i] = -dInfinity;
      //for (i=nub; i<n; i++) hi[i] = 0;
      for (i=nub; i<n; i++) lo[i] = -(dRandReal()*REAL(1.0))-REAL(0.01);
      for (i=nub; i<n; i++) hi[i] =  (dRandReal()*REAL(1.0))+REAL(0.01);

      // set a few limits to lo=hi=0
      /*
      for (i=0; i<10; i++) {
      int j = dRandInt (n-nub) + nub;
      lo[j] = 0;
      hi[j] = 0;
      }
      */

      // solve the LCP. we must make copy of A,b,lo,hi (A2,b2,lo2,hi2) for
      // SolveLCP() to permute. also, we'll clear the upper triangle of A2 to
      // ensure that it doesn't get referenced (if it does, the answer will be
      // wrong).

      memcpy (A2,A,n*nskip*sizeof(dReal));
      dClearUpperTriangle (A2,n);
      memcpy (b2,b,n*sizeof(dReal));
      memcpy (lo2,lo,n*sizeof(dReal));
      memcpy (hi2,hi,n*sizeof(dReal));
      dSetZero (x,n);
      dSetZero (w,n);

      dStopwatch sw;
      dStopwatchReset (&sw);
      dStopwatchStart (&sw);

      dSolveLCP (arena,n,A2,x,b2,w,nub,lo2,hi2,0);

      dStopwatchStop (&sw);
      double time = dStopwatchTime(&sw);
      total_time += time;
      double average = total_time / double(count+1) * 1000.0;

      // check the solution

      dMultiply0 (tmp1,A,x,n,n,1);
      for (i=0; i<n; i++) tmp2[i] = b[i] + w[i];
      dReal diff = dMaxDifference (tmp1,tmp2,n,1);
      // printf ("\tA*x = b+w, maximum difference = %.6e - %s (1)\n",diff,
      //	    diff > tol ? "FAILED" : "passed");
      if (diff > tol) dDebug (0,"A*x = b+w, maximum difference = %.6e",diff);
      int n1=0,n2=0,n3=0;
      for (i=0; i<n; i++) {
        if (x[i]==lo[i] && w[i] >= 0) {
          n1++;	// ok
        }
        else if (x[i]==hi[i] && w[i] <= 0) {
          n2++;	// ok
        }
        else if (x[i] >= lo[i] && x[i] <= hi[i] && w[i] == 0) {
          n3++;	// ok
        }
        else {
          dDebug (0,"FAILED: i=%d x=%.4e w=%.4e lo=%.4e hi=%.4e",i,
            x[i],w[i],lo[i],hi[i]);
        }
      }

      // pacifier
      printf ("passed: NL=%3d NH=%3d C=%3d   ",n1,n2,n3);
      printf ("time=%10.3f ms  avg=%10.4f\n",time * 1000.0,average);
    
    } END_STATE_SAVE(arena, saveInner);
  }
コード例 #14
0
ファイル: rod.cpp プロジェクト: robot-nishida/defense_rod
/*** キー入力関数 ***/
void command(int cmd)
{
  switch(cmd){
  case '0':
    if(act_flg)
      act_flg = false;
    else
      act_flg = true;
    break;
  case 'a':
    // ROD回転(y軸向かって、左方向)
    rod_q_d += 5.0 * M_PI / 180.0;
    break;
  case 's':
    // ROD回転(y軸向かって、右方向)
    rod_q_d -= 2.0 * M_PI / 180.0;
    break;
  case 'e':
    // RODリセット
    rod_q_d = 0.0;
    break;
  case 'r':
    // CANNONリセット
    {
      cannon_q_d[0] = 0.0;
      cannon_q_d[1] = 0.0;
    }
    break;
  case '[':
    // CANNON回転(水平、左右方向)
    cannon_q_d[0] += 0.101 * M_PI / 180.0;
    break;
  case ']':
    // CANNON回転(水平、左右方向)
    cannon_q_d[0] -= 0.101 * M_PI / 180.0;
    break;
  case '1':
    // CANNON回転(仰角、上)
    cannon_q_d[1] += 0.101 * M_PI / 180.0;
    break;
  case '2':
    // CANNON回転(仰角、下)
    cannon_q_d[1] -= 0.101 * M_PI / 180.0;
    break;
  case 'x':
    // CANNON発射
    {
      mytime = 0.0;
      dMatrix3 R2,R3,R4;
      dRFromAxisAndAngle(R2,0,0,1,cannon_q_d[0]);
      dRFromAxisAndAngle(R3,1,0,0,cannon_q_d[1] + 0.5 * M_PI);
      dMultiply0 (R4,R2,R3,3,3,3);
      dReal cpos[3] = {CANNON_X,CANNON_Y,CANNON_Z};
      for(int i=0; i<3; i++){
        cpos[i] += 3*R4[i*4+2];
      }
      dBodySetPosition(bullet.body,cpos[0],cpos[1],cpos[2]);
      dReal force = 500;//500;
      dBodySetLinearVel(bullet.body,force*R4[2],force*R4[6],force*R4[10]);
      dBodySetAngularVel(bullet.body,0,0,0);
      break;
    }
  default:
    break;
  }
}
コード例 #15
0
dReal doStuffAndGetError (int n)
{
  switch (n) {

  // ********** fixed joint

  case 0: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the orientations are the same
    const dReal *R1 = dBodyGetRotation (body[0]);
    const dReal *R2 = dBodyGetRotation (body[1]);
    dReal err1 = dMaxDifference (R1,R2,3,3);
    // check the body offset is correct
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return (err1 + length (pp)) * 300;
  }

  case 1: {			// 1 body to static env
    addOscillatingTorque (0.1);

    // check the orientation is the identity
    dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return (err1 + length (p)) * 1e6;
  }

  case 2: {			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    // check the body offset is correct
    // Should really check body rotation too.  Oh well.
    const dReal *R1 = dBodyGetRotation (body[0]);
    dVector3 p,pp;
    const dReal *p1 = dBodyGetPosition (body[0]);
    const dReal *p2 = dBodyGetPosition (body[1]);
    for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
    dMULTIPLY1_331 (pp,R1,p);
    pp[0] += 0.5;
    pp[1] += 0.5;
    return length(pp) * 300;
  }

  case 3: {			// 1 body to static env with relative rotation
    addOscillatingTorque (0.1);

    // check the body offset is correct
    dVector3 p;
    const dReal *p1 = dBodyGetPosition (body[0]);
    for (int i=0; i<3; i++) p[i] = p1[i];
    p[0] -= 0.25;
    p[1] -= 0.25;
    p[2] -= 1;
    return  length (p) * 1e6;
  }


  // ********** hinge joint

  case 200:			// 2 body
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    return dInfinity;

  case 220:			// hinge angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHingeAngle (joint);
      if (a > 0.5 && a < 1) return 0; else return 10;
    }
    return 0;

  case 221: {			// hinge angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHingeAngle (joint);
    dReal r = dJointGetHingeAngleRate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er) * 4e4;
  }

  case 230:			// hinge motor rate (and polarity) test
  case 231: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHingeAngleRate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHingeParam (joint,dParamVel,cos(a));
    if (n==231) return dInfinity;
    return err * 1e6;
  }

  // ********** slider joint

  case 300:			// 2 body
    addOscillatingTorque (0.05);
    dampRotationalMotion (0.1);
    addSpringForce (0.5);
    return dInfinity;

  case 320:			// slider angle polarity test
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    if (iteration == 40) {
      dReal a = dJointGetSliderPosition (joint);
      if (a > 0.2 && a < 0.5) return 0; else return 10;
      return a;
    }
    return 0;

  case 321: {			// slider angle rate test
    static dReal last_pos = 0;
    dBodyAddForce (body[0],0,0,0.1);
    dBodyAddForce (body[1],0,0,-0.1);
    dReal p = dJointGetSliderPosition (joint);
    dReal r = dJointGetSliderPositionRate (joint);
    dReal er = (p-last_pos)/STEPSIZE;	// estimated rate (almost exact)
    last_pos = p;
    return fabs(r-er) * 1e9;
  }

  case 330:			// slider motor rate (and polarity) test
  case 331: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetSliderPositionRate (joint);
    dReal err = fabs (0.7*cos(a) - r);
    if (a < 0.04) err = 0;
    a += 0.03;
    dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
    if (n==331) return dInfinity;
    return err * 1e6;
  }

  // ********** hinge-2 joint

  case 420:			// hinge-2 steering angle polarity test
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    if (iteration == 40) {
      dReal a = dJointGetHinge2Angle1 (joint);
      if (a > 0.5 && a < 0.6) return 0; else return 10;
    }
    return 0;

  case 421: {			// hinge-2 steering angle rate test
    static dReal last_angle = 0;
    dBodyAddTorque (body[0],0,0,0.01);
    dBodyAddTorque (body[1],0,0,-0.01);
    dReal a = dJointGetHinge2Angle1 (joint);
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal er = (a-last_angle)/STEPSIZE;		// estimated rate
    last_angle = a;
    return fabs(r-er)*2e4;
  }

  case 430:			// hinge 2 steering motor rate (+polarity) test
  case 431: {			// ...with stops
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle1Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel,cos(a));
    if (n==431) return dInfinity;
    return err * 1e6;
  }

  case 432: {			// hinge 2 wheel motor rate (+polarity) test
    static dReal a = 0;
    dReal r = dJointGetHinge2Angle2Rate (joint);
    dReal err = fabs (cos(a) - r);
    if (a==0) err = 0;
    a += 0.03;
    dJointSetHinge2Param (joint,dParamVel2,cos(a));
    return err * 1e6;
  }

  // ********** angular motor joint

  case 600: {			// test euler angle calculations
    // desired euler angles from last iteration
    static dReal a1,a2,a3;

    // find actual euler angles
    dReal aa1 = dJointGetAMotorAngle (joint,0);
    dReal aa2 = dJointGetAMotorAngle (joint,1);
    dReal aa3 = dJointGetAMotorAngle (joint,2);
    // printf ("actual  = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);

    dReal err = dInfinity;
    if (iteration > 0) {
      err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
      err *= 1e10;
    }

    // get random base rotation for both bodies
    dMatrix3 Rbase;
    dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
			3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
    dBodySetRotation (body[0],Rbase);

    // rotate body 2 by random euler angles w.r.t. body 1
    a1 = 3.14 * 2 * (dRandReal()-0.5);
    a2 = 1.57 * 2 * (dRandReal()-0.5);
    a3 = 3.14 * 2 * (dRandReal()-0.5);
    dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
    dRFromAxisAndAngle (R1,0,0,1,-a1);
    dRFromAxisAndAngle (R2,0,1,0,a2);
    dRFromAxisAndAngle (R3,1,0,0,-a3);
    dMultiply0 (Rtmp1,R2,R3,3,3,3);
    dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
    dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
    dBodySetRotation (body[1],Rtmp1);
    // printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);

    return err;
  }

  // ********** universal joint

  case 700: {		// 2 body: joint constraint
    dVector3 ax1, ax2;

    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 701: {		// 2 body: angle 1 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 702: {		// 2 body: angle 2 rate
    static dReal last_angle = 0;
    addOscillatingTorque (0.1);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    // I'm not sure why the error is so large here.
    return fabs(r - er) * 1e1;
  }

  case 720: {		// universal transmit torque test: constraint error
    dVector3 ax1, ax2;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 721: {		// universal transmit torque test: angle1 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 722: {		// universal transmit torque test: angle2 rate
    static dReal last_angle = 0;
    addOscillatingTorqueAbout (0.1, 1, 1, 0);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 730:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 731:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 2e3;
  }

  case 732:{
    dVector3 ax1;
    static dReal last_angle = 0;
    dJointGetUniversalAxis1(joint, ax1);
    addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 740:{
    dVector3 ax1, ax2;
    dJointGetUniversalAxis1(joint, ax1);
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    return fabs(10*dDOT(ax1, ax2));
  }

  case 741:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle1(joint);
    dReal r = dJointGetUniversalAngle1Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e10;
  }

  case 742:{
    dVector3 ax2;
    static dReal last_angle = 0;
    dJointGetUniversalAxis2(joint, ax2);
    addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
    dampRotationalMotion (0.1);
    dReal a = dJointGetUniversalAngle2(joint);
    dReal r = dJointGetUniversalAngle2Rate(joint);
    dReal diff = a - last_angle;
    if (diff > M_PI) diff -= 2*M_PI;
    if (diff < -M_PI) diff += 2*M_PI;
    dReal er = diff / STEPSIZE;    // estimated rate
    last_angle = a;
    return fabs(r - er) * 1e4;
  }
  }

  return dInfinity;
}
コード例 #16
0
ファイル: test_I.cpp プロジェクト: soubok/libset
void reset_test()
{
  int i;
  dMass m,anchor_m;
  dReal q[NUM][3], pm[NUM];	// particle positions and masses
  dReal pos1[3] = {1,0,1};	// point of reference (POR)
  dReal pos2[3] = {-1,0,1};	// point of reference (POR)

  // make random particle positions (relative to POR) and masses
  for (i=0; i<NUM; i++) {
    pm[i] = dRandReal()+0.1;
    q[i][0] = dRandReal()-0.5;
    q[i][1] = dRandReal()-0.5;
    q[i][2] = dRandReal()-0.5;
  }

  // adjust particle positions so centor of mass = POR
  computeMassParams (&m,q,pm);
  for (i=0; i<NUM; i++) {
    q[i][0] -= m.c[0];
    q[i][1] -= m.c[1];
    q[i][2] -= m.c[2];
  }

  if (world) dWorldDestroy (world);
  world = dWorldCreate();

  anchor_body = dBodyCreate (world);
  dBodySetPosition (anchor_body,pos1[0],pos1[1],pos1[2]);
  dMassSetBox (&anchor_m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&anchor_m,0.1);
  dBodySetMass (anchor_body,&anchor_m);

  for (i=0; i<NUM; i++) {
    particle[i] = dBodyCreate (world);
    dBodySetPosition (particle[i],
		      pos1[0]+q[i][0],pos1[1]+q[i][1],pos1[2]+q[i][2]);
    dMassSetBox (&m,1,SIDE,SIDE,SIDE);
    dMassAdjust (&m,pm[i]);
    dBodySetMass (particle[i],&m);
  }

  for (i=0; i < NUM; i++) {
    particle_joint[i] = dJointCreateBall (world,0);
    dJointAttach (particle_joint[i],anchor_body,particle[i]);
    const dReal *p = dBodyGetPosition (particle[i]);
    dJointSetBallAnchor (particle_joint[i],p[0],p[1],p[2]);
  }

  // make test_body with the same mass and inertia of the anchor_body plus
  // all the particles

  test_body = dBodyCreate (world);
  dBodySetPosition (test_body,pos2[0],pos2[1],pos2[2]);
  computeMassParams (&m,q,pm);
  m.mass += anchor_m.mass;
  for (i=0; i<12; i++) m.I[i] = m.I[i] + anchor_m.I[i];
  dBodySetMass (test_body,&m);

  // rotate the test and anchor bodies by a random amount
  dQuaternion qrot;
  for (i=0; i<4; i++) qrot[i] = dRandReal()-0.5;
  dNormalize4 (qrot);
  dBodySetQuaternion (anchor_body,qrot);
  dBodySetQuaternion (test_body,qrot);
  dMatrix3 R;
  dQtoR (qrot,R);
  for (i=0; i<NUM; i++) {
    dVector3 v;
    dMultiply0 (v,R,&q[i][0],3,3,1);
    dBodySetPosition (particle[i],pos1[0]+v[0],pos1[1]+v[1],pos1[2]+v[2]);
  }

  // set random torque
  for (i=0; i<3; i++) torque[i] = (dRandReal()-0.5) * 0.1;


  iteration=0;
}
コード例 #17
0
ファイル: demo_collision.cpp プロジェクト: JohnCrash/ode
int test_box_point_depth()
{
  int i,j;
  dVector3 s,p,q,q2;	// s = box sides
  dMatrix3 R;
  dReal ss,d;		// ss = smallest side

  dSimpleSpace space(0);
  dGeomID box = dCreateBox (0,1,1,1);
  dSpaceAdd (space,box);

  // ********** make a random box

  for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
  dGeomBoxSetLengths (box,s[0],s[1],s[2]);
  dMakeRandomVector (p,3,1.0);
  dGeomSetPosition (box,p[0],p[1],p[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (box,R);

  // ********** test center point has depth of smallest side

  ss = 1e9;
  for (j=0; j<3; j++) if (s[j] < ss) ss = s[j];
  if (dFabs(dGeomBoxPointDepth (box,p[0],p[1],p[2]) - 0.5*ss) > tol)
    FAILED();

  // ********** test point on surface has depth 0

  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
  i = dRandInt (3);
  if (dRandReal() > 0.5) q[i] = 0.5*s[i]; else q[i] = -0.5*s[i];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2])) > tol) FAILED();

  // ********** test points outside box have -ve depth

  for (j=0; j<3; j++) {
    q[j] = 0.5*s[j] + dRandReal() + 0.01;
    if (dRandReal() > 0.5) q[j] = -q[j];
  }
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) >= 0) FAILED();

  // ********** test points inside box have +ve depth

  for (j=0; j<3; j++) q[j] = s[j] * 0.99 * (dRandReal()-0.5);
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  if (dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) <= 0) FAILED();

  // ********** test random depth of point aligned along axis (up to ss deep)

  i = dRandInt (3);
  for (j=0; j<3; j++) q[j] = 0;
  d = (dRandReal()*(ss*0.5+1)-1);
  q[i] = s[i]*0.5 - d;
  if (dRandReal() > 0.5) q[i] = -q[i];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  if (dFabs(dGeomBoxPointDepth (box,q2[0],q2[1],q2[2]) - d) >= tol) FAILED();

  PASSED();
}
コード例 #18
0
static void command (int cmd)
{
	switch (cmd) {
	case 'a': case 'A':
		speed += 0.3;
		break;
	case 'z': case 'Z':
		speed -= 0.3;
		break;
	case ',':
		turn += 0.1;
		if (turn > 0.3)
			turn = 0.3;
		break;
	case '.':
		turn -= 0.1;
		if (turn < -0.3)
			turn = -0.3;
		break;
	case ' ':
		speed = 0;
		turn = 0;
		break;
	case 'f': case 'F':
		doFast = !doFast;
		break;
	case '+':
		dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) + 1);
		break;
	case '-':
		dWorldSetAutoEnableDepthSF1 (world, dWorldGetAutoEnableDepthSF1 (world) - 1);
		break;
	case 'r': case 'R':
		resetSimulation();
		break;
	case '[':
		cannon_angle += 0.1;
		break;
	case ']':
		cannon_angle -= 0.1;
		break;
	case '1':
		cannon_elevation += 0.1;
		break;
	case '2':
		cannon_elevation -= 0.1;
		break;
	case 'x': case 'X': {
		dMatrix3 R2,R3,R4;
		dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
		dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
		dMultiply0 (R4,R2,R3,3,3,3);
		dReal cpos[3] = {CANNON_X,CANNON_Y,1};
		for (int i=0; i<3; i++) cpos[i] += 3*R4[i*4+2];
		dBodySetPosition (cannon_ball_body,cpos[0],cpos[1],cpos[2]);
		dReal force = 10;
		dBodySetLinearVel (cannon_ball_body,force*R4[2],force*R4[6],force*R4[10]);
		dBodySetAngularVel (cannon_ball_body,0,0,0);
		break;
	}
	}
}
コード例 #19
0
ファイル: demo_collision.cpp プロジェクト: JohnCrash/ode
int test_ray_and_box()
{
  int i,j;
  dContactGeom contact;
  dVector3 s,p,q,n,q2,q3,q4;		// s = box sides
  dMatrix3 R;
  dReal k;

  dSimpleSpace space(0);
  dGeomID ray = dCreateRay (0,0);
  dGeomID box = dCreateBox (0,1,1,1);
  dSpaceAdd (space,ray);
  dSpaceAdd (space,box);

  // ********** make a random box

  for (j=0; j<3; j++) s[j] = dRandReal() + 0.1;
  dGeomBoxSetLengths (box,s[0],s[1],s[2]);
  dMakeRandomVector (p,3,1.0);
  dGeomSetPosition (box,p[0],p[1],p[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (box,R);

  // ********** test zero length ray just inside box

  dGeomRaySetLength (ray,0);
  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
  i = dRandInt (3);
  if (dRandReal() > 0.5) q[i] = 0.99*0.5*s[i]; else q[i] = -0.99*0.5*s[i];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (ray,R);
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();

  // ********** test zero length ray just outside box

  dGeomRaySetLength (ray,0);
  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
  i = dRandInt (3);
  if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  dGeomSetPosition (ray,q2[0],q2[1],q2[2]);
  dRFromAxisAndAngle (R,dRandReal()*2-1,dRandReal()*2-1,
		      dRandReal()*2-1,dRandReal()*10-5);
  dGeomSetRotation (ray,R);
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();

  // ********** test finite length ray totally contained inside the box

  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*0.99*s[j];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  for (j=0; j<3; j++) q3[j] = (dRandReal()-0.5)*0.99*s[j];
  dMultiply0 (q4,dGeomGetRotation(box),q3,3,3,1);
  for (j=0; j<3; j++) q4[j] += p[j];
  for (j=0; j<3; j++) n[j] = q4[j] - q2[j];
  dNormalize3 (n);
  dGeomRaySet (ray,q2[0],q2[1],q2[2],n[0],n[1],n[2]);
  dGeomRaySetLength (ray,dCalcPointsDistance3(q2,q4));
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();

  // ********** test finite length ray totally outside the box

  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
  i = dRandInt (3);
  if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q3[j] = q2[j] + p[j];
  dNormalize3 (q2);
  dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
  dGeomRaySetLength (ray,10);
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();

  // ********** test ray from outside to just above surface

  for (j=0; j<3; j++) q[j] = (dRandReal()-0.5)*s[j];
  i = dRandInt (3);
  if (dRandReal() > 0.5) q[i] = 1.01*0.5*s[i]; else q[i] = -1.01*0.5*s[i];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q3[j] = 2*q2[j] + p[j];
  k = dSqrt(q2[0]*q2[0] + q2[1]*q2[1] + q2[2]*q2[2]);
  for (j=0; j<3; j++) q2[j] = -q2[j];
  dGeomRaySet (ray,q3[0],q3[1],q3[2],q2[0],q2[1],q2[2]);
  dGeomRaySetLength (ray,k*0.99);
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 0) FAILED();

  // ********** test ray from outside to just below surface

  dGeomRaySetLength (ray,k*1.01);
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom)) != 1) FAILED();

  // ********** test contact point position for random rays

  for (j=0; j<3; j++) q[j] = dRandReal()*s[j];
  dMultiply0 (q2,dGeomGetRotation(box),q,3,3,1);
  for (j=0; j<3; j++) q2[j] += p[j];
  for (j=0; j<3; j++) q3[j] = dRandReal()-0.5;
  dNormalize3 (q3);
  dGeomRaySet (ray,q2[0],q2[1],q2[2],q3[0],q3[1],q3[2]);
  dGeomRaySetLength (ray,10);
  if (dCollide (ray,box,1,&contact,sizeof(dContactGeom))) {
    // check depth of contact point
    if (dFabs (dGeomBoxPointDepth
	       (box,contact.pos[0],contact.pos[1],contact.pos[2])) > tol)
      FAILED();
    // check position of contact point
    for (j=0; j<3; j++) contact.pos[j] -= p[j];
    dMultiply1 (q,dGeomGetRotation(box),contact.pos,3,3,1);
    if ( dFabs(dFabs (q[0]) - 0.5*s[0]) > tol &&
	 dFabs(dFabs (q[1]) - 0.5*s[1]) > tol &&
	 dFabs(dFabs (q[2]) - 0.5*s[2]) > tol) {
      FAILED();
    }
    // also check normal signs
    if (dCalcVectorDot3 (q3,contact.normal) > 0) FAILED();

    draw_all_objects (space);
  }

  PASSED();
}
コード例 #20
0
static void simLoop (int pause)
{
	int i, j;
		
	dsSetTexture (DS_WOOD);

	if (!pause) {
#ifdef BOX
		dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
		for (j = 0; j < joints; j++)
		{
			dReal curturn = dJointGetHinge2Angle1 (joint[j]);
			//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
			dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
			dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
			dJointSetHinge2Param(joint[j],dParamVel2,speed);
			dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
			dBodyEnable(dJointGetBody(joint[j],0));
			dBodyEnable(dJointGetBody(joint[j],1));
		}		
		if (doFast)
		{
			dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
			dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
			dWorldStepFast1 (world,0.05,ITERS);
#endif
			dJointGroupEmpty (contactgroup);
		}
		else
		{
			dSpaceCollide (space,0,&nearCallback);
			dWorldStep (world,0.05);
			dJointGroupEmpty (contactgroup);
		}
		
		for (i = 0; i < wb; i++)
		{
			b = dGeomGetBody(wall_boxes[i]);
			if (dBodyIsEnabled(b)) 
			{
				bool disable = true;
				const dReal *lvel = dBodyGetLinearVel(b);
				dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
				if (lspeed > DISABLE_THRESHOLD)
					disable = false;
				const dReal *avel = dBodyGetAngularVel(b);
				dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
				if (aspeed > DISABLE_THRESHOLD)
					disable = false;
				
				if (disable)
					wb_stepsdis[i]++;
				else
					wb_stepsdis[i] = 0;
				
				if (wb_stepsdis[i] > DISABLE_STEPS)
				{
					dBodyDisable(b);
					dsSetColor(0.5,0.5,1);
				}
				else
					dsSetColor(1,1,1);

			}
			else
				dsSetColor(0.4,0.4,0.4);
			dVector3 ss;
			dGeomBoxGetLengths (wall_boxes[i], ss);
			dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
		}
	}
	else
	{
		for (i = 0; i < wb; i++)
		{
			b = dGeomGetBody(wall_boxes[i]);
			if (dBodyIsEnabled(b))
				dsSetColor(1,1,1);
			else
				dsSetColor(0.4,0.4,0.4);
			dVector3 ss;
			dGeomBoxGetLengths (wall_boxes[i], ss);
			dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
		}
	}
	
	dsSetColor (0,1,1);
	dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
	for (i = 0; i < boxes; i++)
		dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
	dsSetColor (1,1,1);
	for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
				   dGeomGetRotation(sphere[i]),RADIUS);
	
	// draw the cannon
	dsSetColor (1,1,0);
	dMatrix3 R2,R3,R4;
	dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
	dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
	dMultiply0 (R4,R2,R3,3,3,3);
	dReal cpos[3] = {CANNON_X,CANNON_Y,1};
	dReal csides[3] = {2,2,2};
	dsDrawBox (cpos,R2,csides);
	for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
	dsDrawCylinder (cpos,R4,3,0.5);
	
	// draw the cannon ball
	dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
		      CANNON_BALL_RADIUS);
}
コード例 #21
0
ファイル: demo_ode.cpp プロジェクト: Devilmore/GoalBabbling
void testMassFunctions()
{
  dMass m;
  int i,j;
  dReal q[NUMP][3];		// particle positions
  dReal pm[NUMP];		// particle masses
  dMass m1,m2;
  dMatrix3 R;

  HEADER;

  printf ("\t");
  dMassSetZero (&m);
  TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6),
		printf (" FAILED (1)\n"), printf (" passed (1)\n"));

  printf ("\t");
  dMassSetZero (&m);
  TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4),
		printf ("passed (2)\n") , printf (" FAILED (2)\n"));
  if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) &&
      m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 &&
      m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 &&
      m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4)
    printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n");

  dMassSetZero (&m);
  dMassSetSphere (&m,1.4, 0.86);
  if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
      cmp(m._I(0,0),1.10349124669826) &&
      cmp(m._I(1,1),1.10349124669826) &&
      cmp(m._I(2,2),1.10349124669826) &&
      m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
      m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
    printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n");

  dMassSetZero (&m);
  dMassSetCapsule (&m,1.3,1,0.76,1.53);
  if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
      cmp(m._I(0,0),1.59461986077384) &&
      cmp(m._I(1,1),4.21878433864904) &&
      cmp(m._I(2,2),4.21878433864904) &&
      m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
      m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
    printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n");

  dMassSetZero (&m);
  dMassSetBox (&m,0.27,3,4,5);
  if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 &&
      cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) &&
      m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 &&
      m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0)
    printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n");

  // test dMassAdjust?

  // make random particles and compute the mass, COM and inertia, then
  // translate and repeat.
  for (i=0; i<NUMP; i++) {
    pm[i] = dRandReal()+0.5;
    for (j=0; j<3; j++) {
      q[i][j] = 2.0*(dRandReal()-0.5);
    }
  }
  computeMassParams (&m1,q,pm);
  memcpy (&m2,&m1,sizeof(dMass));
  dMassTranslate (&m2,1,2,-3);
  for (i=0; i<NUMP; i++) {
    q[i][0] += 1;
    q[i][1] += 2;
    q[i][2] -= 3;
  }
  computeMassParams (&m1,q,pm);
  compareMassParams (&m1,&m2,"7");

  // rotate the masses
  _R(0,0) = -0.87919618797635;
  _R(0,1) = 0.15278881840384;
  _R(0,2) = -0.45129772879842;
  _R(1,0) = -0.47307856232664;
  _R(1,1) = -0.39258064912909;
  _R(1,2) = 0.78871864932708;
  _R(2,0) = -0.05666336483842;
  _R(2,1) = 0.90693771059546;
  _R(2,2) = 0.41743652473765;
  dMassRotate (&m2,R);
  for (i=0; i<NUMP; i++) {
    dReal a[3];
    dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1);
    q[i][0] = a[0];
    q[i][1] = a[1];
    q[i][2] = a[2];
  }
  computeMassParams (&m1,q,pm);
  compareMassParams (&m1,&m2,"8");
}