コード例 #1
0
ファイル: TSRODERigidBody.cpp プロジェクト: ShadyEM/Twister3D
void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
	TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
    dMass totalMass;
    dBodyGetMass( m_BodyID, &totalMass );
    if ( m_GeomIDs.size() == 0 )
    {
        dMassSetZero( &totalMass );
    }
    dMatrix4 R;
    dVector3 P;
    Matrix4ToODE( _bodyToGeomTransform, R, P );
    dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
    dGeomID encapsulatedGeom = 0;
    dMass currMass;
    dMassSetZero( &currMass );

    encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );

    dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
    dMassRotate( &currMass, R );
    //dMassTranslate(&currMass,P[0],P[1],P[2]);
    dMassAdd( &totalMass, &currMass );
    dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
    dGeomSetRotation( encapsulatedGeom, R );
    dGeomTransformSetCleanup( geomTransform, 1 );
    dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
    dGeomSetBody( geomTransform, m_BodyID );

    m_GeomIDs.push_back( geomTransform );
    dBodySetMass( m_BodyID, &totalMass );

}
コード例 #2
0
ファイル: Shape.cpp プロジェクト: moltenguy1/deusexmachina
// Universal method for all specific ODE geom types, which add the
// geom to the collide space, using an ODE proxy geom to offset the
// geom by the provided transformation matrix. The geom will also
// be attached to the rigid body, if any is set.
void CShape::AttachGeom(dGeomID GeomId, dSpaceID SpaceID)
{
	n_assert(GeomId);
	n_assert(!IsAttached());

	// set the geom's local Transform
	const vector3& Pos = Transform.pos_component();
	dGeomSetPosition(GeomId, Pos.x, Pos.y, Pos.z);
	dMatrix3 ODERotation;
	CPhysicsServer::Matrix44ToOde(Transform, ODERotation);
	dGeomSetRotation(GeomId, ODERotation);

	// if attached to rigid body, create a geom Transform "proxy" object && attach it to the rigid body
	// else directly set Transform and rotation
	if (pRigidBody)
	{
		ODEGeomID = dCreateGeomTransform(0);
		dGeomTransformSetCleanup(ODEGeomID, 1);
		dGeomTransformSetGeom(ODEGeomID, GeomId);
		dGeomSetBody(ODEGeomID, pRigidBody->GetODEBodyID());
	}
	else ODEGeomID = GeomId;

	dGeomSetCategoryBits(ODEGeomID, CatBits);
	dGeomSetCollideBits(ODEGeomID, CollBits);
	dGeomSetData(ODEGeomID, this);
	AttachToSpace(SpaceID);
}
コード例 #3
0
void CODEGeom::init()
{
	dGeomID	geom=create();
	m_geom_transform=dCreateGeomTransform(0);
	dGeomTransformSetCleanup(m_geom_transform,0);
	dGeomSetData(m_geom_transform,0);
	dGeomTransformSetGeom(m_geom_transform,geom);
	dGeomTransformSetInfo(m_geom_transform,1);
	dGeomCreateUserData(geom);
	dGeomUserDataSetBoneId(geom,m_bone_id);
}
コード例 #4
0
void CODEGeom::destroy()
{
	if(!m_geom_transform) return;
	if(geom())
	{
		dGeomDestroyUserData(geom());
		dGeomDestroy(geom());
		dGeomTransformSetGeom(m_geom_transform,0);
	}
	dGeomDestroyUserData(m_geom_transform);
	dGeomDestroy(m_geom_transform);
	m_geom_transform=NULL;
}
コード例 #5
0
ファイル: rigidcapsule.cpp プロジェクト: harkal/sylphis3d
CRigidCapsule::CRigidCapsule(S32 parent, CSceneObject *so, const CVector3 &d ) : CRigidBody(parent, so) {
	if(parent == 0)parent = (S32)space;

#ifdef SIM
	mGeomID = dCreateGeomTransform ((dSpaceID)parent);
	dGeomTransformSetCleanup (mGeomID, 1);
	F32 radius = sqrt(d.x * d.x + d.z * d.z) / 2.0;
	//F32 radius = d.x / 2;
	dGeomID mGeomSphere = dCreateSphere(0, TO_PHYSICS(radius));
	dGeomTransformSetGeom (mGeomID, mGeomSphere);
	dGeomSetPosition (mGeomSphere, 0, TO_PHYSICS(-d.y/2 + radius), 0);

	mGeomID2 = dCreateGeomTransform ((dSpaceID)parent);
	dGeomTransformSetCleanup (mGeomID2, 1);
	dGeomID mGeomBox = dCreateBox(0, TO_PHYSICS(d.x), TO_PHYSICS(d.y - radius), TO_PHYSICS(d.z));
	dGeomTransformSetGeom (mGeomID2, mGeomBox);
	dGeomSetPosition (mGeomBox, 0, TO_PHYSICS(radius / 2), 0);
#else
    F32 length = d.y - d.x * 2;
    mGeomID = dCreateCCylinder((dSpaceID)parent, TO_PHYSICS(d.x), TO_PHYSICS(length));
#endif

	mBodyID = dBodyCreate(world);
	dGeomSetBody(mGeomID, mBodyID);
#ifdef SIM
	dGeomSetBody(mGeomID2, mBodyID);
#endif
	dGeomSetData(mGeomID, static_cast<void *>(this));
#ifdef SIM
	dGeomSetData(mGeomID2, static_cast<void *>(this));
#endif
	mDimentions = d;
    mDimentions.y = length;
	setDensity(0.0001);
    dBodySetAutoDisableFlag(mBodyID, 0);

	mGroundBox.setBounds(CVector3(-d.x, -d.y / 2 - 5.0, -d.z), CVector3(d.x, -d.y / 2 + 20.0, d.z));
}
コード例 #6
0
        void LaserBeam::createGeometry(const dSpaceID& space)
        {
          /// build a ray
          Model::LaserBeam* beam = getTrait<Model::LaserBeam>() ;
          Model::Mobile* mobile = getTrait<Model::Mobile>() ;

          float collision_beam_length =
              std::max(mobile->getSpeed().MeterPerSecond().length()*getControlerSet()->getTimeStep(),
                       beam->getLength().Meter()) ;

          m_ray = dCreateRay(0,2*collision_beam_length) ;

          /// we use -z as forward @see Model::Orientation
          dGeomRaySet(m_ray,0,0,0,0,0,-1) ;
          dGeomRaySetParams(m_ray,false,false) ;
          dGeomRaySetClosestHit(m_ray,true) ;

          /// a geometry transform allows to have a local z axis inversion
          m_geometry1 = dCreateGeomTransform(space) ;
          dGeomTransformSetGeom(m_geometry1,m_ray) ;
          dGeomSetCollideBits(m_geometry1,(unsigned long)Collideable::Laser) ;
        }
コード例 #7
0
  void Transform::init(const OdeHandle& odeHandle, double mass, const OsgHandle& osgHandle,
                       char mode) {
    // Primitive::body is ignored (removed) from mode
    assert(parent && parent->getBody() != 0 && child); // parent and child must exist
    assert(child->getBody() == 0 && child->getGeom() == 0); // child should not be initialised
    this->mode = mode | Primitive::_Transform;
    if (!substanceManuallySet)
      substance = odeHandle.substance;

    QMP_CRITICAL(6);
    // our own geom is just a transform
    geom = dCreateGeomTransform(odeHandle.space);
    dGeomTransformSetInfo(geom, 1);
    dGeomTransformSetCleanup(geom, 0);

    // the child geom must go into space 0 (because it inherits the space from the transform geom)
    OdeHandle odeHandleChild(odeHandle);
    odeHandleChild.space = 0;
    // the root node for the child is the transform node of the parent
    OsgHandle osgHandleChild(osgHandle);
    osgHandleChild.parent = parent->getOSGPrimitive()->getTransform();
    assert(osgHandleChild.scene);
    // initialise the child
    child->init(odeHandleChild, mass, osgHandleChild, (mode & ~Primitive::Body) | Primitive::_Child );
    // move the child to the right place (in local coordinates)
    child->setPose(pose);

    // assoziate the child with the transform geom
    dGeomTransformSetGeom (geom, child->getGeom());
    // finally bind the transform the body of parent
    dGeomSetBody (geom, parent->getBody());
    dGeomSetData(geom, (void*)this); // set primitive as geom data

    // we assign the body here. Since our mode is Transform it is not destroyed
    body=parent->getBody();

    QMP_END_CRITICAL(6);
  }
コード例 #8
0
ファイル: Ball.cpp プロジェクト: unicodon/haribote
void CBall::Init()
{
	static float dif_orange[4] = {0.75,0.4,0.2,1.0};
	static float amb_orange[4] = {0.5,0.2,0.1,1.0};
	static float spec[4] = {1.0,1.0,1.0,1.0};

	glPushMatrix();
	glLoadIdentity();
	glTranslatef(0.000,  0.000,  1.100);
	glGetDoublev(GL_MODELVIEW_MATRIX, position);
	glPopMatrix();

	material.SetDiffuse(dif_orange);
	material.SetAmbient(amb_orange);
	material.SetSpecular(spec);
	material.SetShininess(300);

	radius = 0.040;

	//body設定
	bBall = dBodyCreate(world);
	dBodySetAutoDisableFlag(bBall,0);
	dBodySetPosition(bBall, position[12+0], position[12+1], position[12+2]);

	dMass mass;
	dMassSetSphereTotal(&(mass),0.010,0.040);
	dBodySetMass (bBall ,&(mass));

	//geom設定
	transBall = dCreateGeomTransform(space);
	gBall = dCreateSphere (0,0.040);
	dGeomSetPosition(gBall, 0 ,0, 0.002);//重心が3mmずれてる
	dGeomTransformSetGeom(transBall, gBall);

	dGeomSetBody(transBall, bBall);

}
コード例 #9
0
static void command (int cmd)
{
  int i,j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*)(size_t)i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
    else if (cmd == 'y') {
      sides[1] *= 0.5;
      dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
	else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'm') {
      dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate();
      dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, 
		  (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex));

      obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0);

      // remember the mesh's dTriMeshDataID on its userdata for convenience.
      dGeomSetData(obj[i].geom[0], new_tmdata);

      dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] );
      printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]);
      dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]);
      dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCapsule (0,radius,length);
	  dMassSetCapsule (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
}
コード例 #10
0
ファイル: demo_jointPU.cpp プロジェクト: Belxjander/Asuna
int main (int argc, char **argv)
{
  // setup pointers to drawstuff callback functions
  dsFunctions fn;
  fn.version = DS_VERSION;
  fn.start = &start;
  fn.step = &simLoop;
  fn.command = &command;
  fn.stop = 0;
  fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH;

  if (argc >= 2 ) {
    for (int i=1; i < argc; ++i) {
      if (  0 == strcmp ("-h", argv[i]) || 0 == strcmp ("--help", argv[i]) )
        Help (argv);

      if (  0 == strcmp ("-p", argv[i]) || 0 == strcmp ("--PRJoint", argv[i]) )
        type = dJointTypePR;

      if (0 == strcmp ("-t", argv[i]) || 0 == strcmp ("--texture-path", argv[i]) ) {
        int j = i+1;
        if ( j+1 > argc      ||  // Check if we have enough arguments
             argv[j] == '\0' ||  // We should have a path here
             argv[j][0] == '-' ) // We should have a path not a command line
          Help (argv);
        else
          fn.path_to_textures = argv[++i]; // Increase i since we use this argument
      }
    }
  }

  dInitODE2(0);

  world.setERP (0.8);

  space = dSimpleSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  geom[GROUND] = dCreatePlane (space, 0,0,1,0);
  dGeomSetCategoryBits (geom[GROUND], catBits[GROUND]);
  dGeomSetCollideBits (geom[GROUND], catBits[ALL]);

  dMass m;

  // Create the body attached to the World
  body[W].create (world);
  // Main axis of cylinder is along X=1
  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
  m.adjust (Mass1);
  geom[W] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
  dGeomSetBody (geom[W], body[W]);
  dGeomSetCategoryBits (geom[W], catBits[W]);
  dGeomSetCollideBits (geom[W], catBits[ALL] & (~catBits[W]) & (~catBits[JOINT]) );
  body[W].setMass(m);





  // Create the dandling body
  body[D].create(world);
  // Main axis of capsule is along X=1
  m.setBox (1, boxDim[X], boxDim[Y], boxDim[Z]);
  m.adjust (Mass1);
  geom[D] = dCreateBox (space, boxDim[X], boxDim[Y], boxDim[Z]);
  dGeomSetBody (geom[D], body[D]);
  dGeomSetCategoryBits (geom[D], catBits[D]);
  dGeomSetCollideBits (geom[D], catBits[ALL] & (~catBits[D]) & (~catBits[JOINT]) );
  body[D].setMass(&m);


  // Create the external part of the slider joint
  geom[EXT] = dCreateBox (space, extDim[X], extDim[Y], extDim[Z]);
  dGeomSetCategoryBits (geom[EXT], catBits[EXT]);
  dGeomSetCollideBits (geom[EXT],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );

  // Create the internal part of the slider joint
  geom[INT] = dCreateBox (space, INT_EXT_RATIO*extDim[X],
                          INT_EXT_RATIO*extDim[Y],
                          INT_EXT_RATIO*extDim[Z]);
  dGeomSetCategoryBits (geom[INT], catBits[INT]);
  dGeomSetCollideBits (geom[INT],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );


  dMatrix3 R;
  dGeomID id;
  // Create the first axis of the universal joi9nt
  geom[AXIS1] = dCreateGeomTransform (space);
  //Rotation of 90deg around y
  dRFromAxisAndAngle (R, 0,1,0, 0.5*PI);
  dGeomSetRotation (geom[AXIS1], R);
  dGeomSetCategoryBits (geom[AXIS1], catBits[AXIS1]);
  dGeomSetCollideBits (geom[AXIS1],
                       catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
  id = geom[AXIS1];
  dGeomTransformSetGeom (geom[AXIS1],  dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );


  // Create the second axis of the universal joint
  geom[AXIS2] = dCreateGeomTransform (space);
  //Rotation of 90deg around y
  dRFromAxisAndAngle (R, 1,0,0, 0.5*PI);
  dGeomSetRotation (geom[AXIS2], R);
  dGeomSetCategoryBits (geom[AXIS2], catBits[AXIS2]);
  dGeomSetCollideBits (geom[AXIS2],
                       catBits[ALL]  & ~catBits[JOINT] & ~catBits[W] & ~catBits[D]);
  id = geom[AXIS2];
  dGeomTransformSetGeom (geom[AXIS2],  dCreateCylinder (0, axDim[RADIUS], axDim[LENGTH]) );


  // Create the anchor
  geom[ANCHOR] = dCreateBox (space, ancDim[X], ancDim[Y], ancDim[Z]);
  dGeomSetCategoryBits (geom[ANCHOR], catBits[ANCHOR]);
  dGeomSetCollideBits (geom[ANCHOR],
                       catBits[ALL] & (~catBits[JOINT]) & (~catBits[W]) & (~catBits[D]) );



  if (body[W]) {
    body[W].setPosition(0, 0, 5);
  }


  if (geom[EXT]) {
    dGeomSetPosition (geom[EXT], 0,0,3.8);
  }
  if (geom[INT]) {
    dGeomSetPosition (geom[INT], 0,0,2.6);
  }
  if (geom[AXIS1]) {
    dGeomSetPosition (geom[AXIS1], 0,0,2.5);
  }
  if (geom[AXIS2]) {
    dGeomSetPosition (geom[AXIS2], 0,0,2.5);
  }

  if (geom[ANCHOR]) {
    dGeomSetPosition (geom[ANCHOR], 0,0,2.25);
  }

  if (body[D]) {
    body[D].setPosition(0,0,1.5);
  }



  // Attache the upper box to the world
  dJointID fixed = dJointCreateFixed (world,0);
  dJointAttach (fixed , NULL, body[W]);
  dJointSetFixed (fixed );

  if (type == dJointTypePR) {
    dPRJoint *pr = new dPRJoint (world, 0);
    pr->attach (body[W], body[D]);
    pr->setAxis1 (0, 0, -1);
    pr->setAxis2 (1, 0, 0);
    joint = pr;

    dJointSetPRAnchor (pr->id(), 0, 0, 2.5);
  }
  else {
    dPUJoint *pu = new dPUJoint (world, 0);
    pu->attach (body[W], body[D]);
    pu->setAxis1 (1, 0, 0);
    pu->setAxis2 (0, 1, 0);
    pu->setAxisP (0, 0, -1);
    joint = pu;

    dJointSetPUAnchor (pu->id(), 0, 0, 2.5);
  }


  // run simulation
  dsSimulationLoop (argc,argv,400,300,&fn);

  delete joint;
  dJointGroupDestroy (contactgroup);
  dSpaceDestroy (space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
コード例 #11
0
static void command (int cmd)
{
  size_t i;
  int j,k;
  dReal sides[3];
  dMass m;

  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x'
      /* || cmd == 'l' */) {
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) {
      dBodySetPosition (obj[i].body,
			dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
      dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			  dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
    }
    else {
      dReal maxheight = 0;
      for (k=0; k<num; k++) {
	const dReal *pos = dBodyGetPosition (obj[k].body);
	if (pos[2] > maxheight) maxheight = pos[2];
      }
      dBodySetPosition (obj[i].body, 0,0,maxheight+1);
      dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0);
    }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*) i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]);
    }
/*
    // cylinder option not yet implemented
    else if (cmd == 'l') {
      sides[1] *= 0.5;
      dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
*/
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCCylinder (0,radius,length);
	  dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<2; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
    }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
  else if (cmd == '1') {
    write_world = 1;
  }
}
コード例 #12
0
ファイル: demo_boxstack.cpp プロジェクト: 4nakin/awesomeball
static void command (int cmd)
{
  size_t i;
  int j,k;
  dReal sides[3];
  dMass m;
  int setBody;
  
  cmd = locase (cmd);
  if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v')
  {
    setBody = 0;
    if (num < NUM) {
      i = num;
      num++;
    }
    else {
      i = nextobj;
      nextobj++;
      if (nextobj >= num) nextobj = 0;

      // destroy the body and geoms for slot i
      dBodyDestroy (obj[i].body);
      for (k=0; k < GPB; k++) {
	if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
      }
      memset (&obj[i],0,sizeof(obj[i]));
    }

    obj[i].body = dBodyCreate (world);
    for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1;

    dMatrix3 R;
    if (random_pos) 
      {
	dBodySetPosition (obj[i].body,
			  dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2);
	dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
      }
    else 
      {
	dReal maxheight = 0;
	for (k=0; k<num; k++) 
	  {
	    const dReal *pos = dBodyGetPosition (obj[k].body);
	    if (pos[2] > maxheight) maxheight = pos[2];
	  }
	dBodySetPosition (obj[i].body, 0,0,maxheight+1);
	dRSetIdentity (R);
	//dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0);
      }
    dBodySetRotation (obj[i].body,R);
    dBodySetData (obj[i].body,(void*) i);

    if (cmd == 'b') {
      dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
      obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]);
    }
    else if (cmd == 'c') {
      sides[0] *= 0.5;
      dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]);
    }
    //<---- Convex Object    
    else if (cmd == 'v') 
      {
	dMassSetBox (&m,DENSITY,0.25,0.25,0.25);
#if 0
	obj[i].geom[0] = dCreateConvex (space,
					planes,
					planecount,
					points,
					pointcount,
					polygons);
#else
	obj[i].geom[0] = dCreateConvex (space,
					Sphere_planes,
					Sphere_planecount,
					Sphere_points,
					Sphere_pointcount,
					Sphere_polygons);
#endif
      }
    //----> Convex Object
    else if (cmd == 'y') {
      dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]);
      obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]);
    }
    else if (cmd == 's') {
      sides[0] *= 0.5;
      dMassSetSphere (&m,DENSITY,sides[0]);
      obj[i].geom[0] = dCreateSphere (space,sides[0]);
    }
    else if (cmd == 'x' && USE_GEOM_OFFSET) {
      setBody = 1;
      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries
      dMatrix3 drot[GPB];
      
      // set random delta positions
      for (j=0; j<GPB; j++) {
		for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }
    
      for (k=0; k<GPB; k++) {
		if (k==0) {
		  dReal radius = dRandReal()*0.25+0.05;
		  obj[i].geom[k] = dCreateSphere (space,radius);
		  dMassSetSphere (&m2,DENSITY,radius);
		}
		else if (k==1) {
		  obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]);
		  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
		}
		else {
		  dReal radius = dRandReal()*0.1+0.05;
		  dReal length = dRandReal()*1.0+0.1;
		  obj[i].geom[k] = dCreateCapsule (space,radius,length);
		  dMassSetCapsule (&m2,DENSITY,3,radius,length);
		}

		dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
					dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
		dMassRotate (&m2,drot[k]);
		
		dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);

		// add to the total mass
		dMassAdd (&m,&m2);
		
	}
      for (k=0; k<GPB; k++) {
		dGeomSetBody (obj[i].geom[k],obj[i].body);
		dGeomSetOffsetPosition (obj[i].geom[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
		dGeomSetOffsetRotation(obj[i].geom[k], drot[k]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
	  dBodySetMass (obj[i].body,&m);
		
    }
    else if (cmd == 'x') {
      dGeomID g2[GPB];		// encapsulated geometries
      dReal dpos[GPB][3];	// delta-positions for encapsulated geometries

      // start accumulating masses for the encapsulated geometries
      dMass m2;
      dMassSetZero (&m);

      // set random delta positions
      for (j=0; j<GPB; j++) {
	for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15;
      }

      for (k=0; k<GPB; k++) {
	obj[i].geom[k] = dCreateGeomTransform (space);
	dGeomTransformSetCleanup (obj[i].geom[k],1);
	if (k==0) {
	  dReal radius = dRandReal()*0.25+0.05;
	  g2[k] = dCreateSphere (0,radius);
	  dMassSetSphere (&m2,DENSITY,radius);
	}
	else if (k==1) {
	  g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]);
	  dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
	}
	else {
	  dReal radius = dRandReal()*0.1+0.05;
	  dReal length = dRandReal()*1.0+0.1;
	  g2[k] = dCreateCapsule (0,radius,length);
	  dMassSetCapsule (&m2,DENSITY,3,radius,length);
	}
	dGeomTransformSetGeom (obj[i].geom[k],g2[k]);

	// set the transformation (adjust the mass too)
	dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
	dMatrix3 Rtx;
	dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
			    dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
	dGeomSetRotation (g2[k],Rtx);
	dMassRotate (&m2,Rtx);

	// Translation *after* rotation
	dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);

	// add to the total mass
	dMassAdd (&m,&m2);
      }

      // move all encapsulated objects so that the center of mass is (0,0,0)
      for (k=0; k<GPB; k++) {
	dGeomSetPosition (g2[k],
			  dpos[k][0]-m.c[0],
			  dpos[k][1]-m.c[1],
			  dpos[k][2]-m.c[2]);
      }
      dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
    }

    if (!setBody)
     for (k=0; k < GPB; k++) {
      if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
     }

    dBodySetMass (obj[i].body,&m);
  }

  if (cmd == ' ') {
    selected++;
    if (selected >= num) selected = 0;
    if (selected < 0) selected = 0;
  }
  else if (cmd == 'd' && selected >= 0 && selected < num) {
    dBodyDisable (obj[selected].body);
  }
  else if (cmd == 'e' && selected >= 0 && selected < num) {
    dBodyEnable (obj[selected].body);
  }
  else if (cmd == 'a') {
    show_aabb ^= 1;
  }
  else if (cmd == 't') {
    show_contacts ^= 1;
  }
  else if (cmd == 'r') {
    random_pos ^= 1;
  }
  else if (cmd == '1') {
    write_world = 1;
  }
  else if (cmd == 'p'&& selected >= 0)
  {
    const dReal* pos = dGeomGetPosition(obj[selected].geom[0]);
    const dReal* rot = dGeomGetRotation(obj[selected].geom[0]);
    printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]);
    printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n",
           rot[0],rot[1],rot[2],rot[3],
           rot[4],rot[5],rot[6],rot[7],
           rot[8],rot[9],rot[10],rot[11]);
  }
  else if (cmd == 'f' && selected >= 0 && selected < num) {
          if (dBodyIsEnabled(obj[selected].body))
            doFeedback = 1;
  }
}
コード例 #13
0
ファイル: machine.cpp プロジェクト: Eliasvan/machineball
void Machine::init()
{
	int i;
	pushtime=0;
	energy=4;
	dMass m;
	for(i=0; i<3; i++)
	{
		wheel[i] = dBodyCreate(world);
		dMassSetSphere(&m, 1, 5);
		dMassAdjust(&m, 2);
		dBodySetMass(wheel[i], &m);
		sphere[i] = dCreateSphere(0, 5);
		dGeomSetBody(sphere[i], wheel[i]);
	}
	dBodySetPosition(wheel[0], 0, 12, 6);
	dBodySetPosition(wheel[1], -6, -7, 6);
	dBodySetPosition(wheel[2], 6, -7, 6);

	body[0] = dBodyCreate(world);
	dMassSetBox(&m, 1, 20, 80, 5);
	dMassAdjust(&m, 5);
	dBodySetMass(body[0], &m);
	dBodySetPosition(body[0], 0, 0, 6.5);
	geom[0] = dCreateBox(0, 19, 27, 10);
	dGeomSetBody(geom[0], body[0]);

	body[1] = dBodyCreate(world);
	dMassSetBox(&m, 1, 11, 5, 10);
	dMassAdjust(&m, 0.3);
	dBodySetMass(body[1], &m);
	dBodySetPosition(body[1], 0, 17, 6.5);
	geom[1] = dCreateBox(0, 11, 5, 10);
	dGeomSetBody(geom[1], body[1]);

	joint = dJointCreateSlider(world, 0);
	dJointAttach(joint, body[0], body[1]);
	dJointSetSliderAxis(joint, 0, 1, 0);
	dJointSetSliderParam(joint, dParamLoStop, -9);
	dJointSetSliderParam(joint, dParamHiStop, 0);

	for(i=0; i<2; i++)
	{
		geom[i+2] = dCreateGeomTransform(0);
		dGeomTransformSetCleanup(geom[i+2], 1);
		
		finE[i] = dCreateBox(0, 7, 5, 10);
		dGeomSetPosition(finE[i], i==0?-6.3:6.3, -2, 0);
		dMatrix3 R;
		dRFromAxisAndAngle(R, 0, 0, 1, i==0?M_PI/4:-M_PI/4);
		dGeomSetRotation(finE[i], R);
		dGeomTransformSetGeom(geom[i+2], finE[i]);
		dGeomSetBody(geom[i+2], body[1]);
	}
	
	
	for(i=0; i<3; i++)
	{
		wheeljoint[i] = dJointCreateHinge2(world, 0);
		dJointAttach(wheeljoint[i], body[0], wheel[i]);
		const dReal *wPos = dBodyGetPosition(wheel[i]);
		dJointSetHinge2Anchor(wheeljoint[i], wPos[0], wPos[1], wPos[2]);
		dJointSetHinge2Axis1(wheeljoint[i], 0, 0, 1);
		dJointSetHinge2Axis2(wheeljoint[i], 1, 0, 0);

		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionERP, 0.8);
		dJointSetHinge2Param(wheeljoint[i], dParamSuspensionCFM, 0.01);
		dJointSetHinge2Param(wheeljoint[i], dParamLoStop, 0);
		dJointSetHinge2Param(wheeljoint[i], dParamHiStop, 0);
		dJointSetHinge2Param(wheeljoint[i], dParamCFM, 0.0001);
		dJointSetHinge2Param(wheeljoint[i], dParamStopERP, 0.8);
		dJointSetHinge2Param(wheeljoint[i], dParamStopCFM, 0.0001);
	}

	reset();
}