WheelPiece::WheelPiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
    dBodyCreate(world);
    body = dBodyCreate(world);

    radius = .35;
    thickness = .25;
    activationDirection = 0;

    geom = dCreateCylinder(space, radius, thickness);
    dGeomSetBody(geom, body);
    dGeomSetData(geom, this);
    dMass mass;
    mass.setCylinder(.5, 1, radius, thickness);
    dBodySetMass(body, &mass);

    const dMatrix3 rotationMatrix = {1,0,0,0,
                                     0,1,0,0,
                                     0,0,1,0
                                    };

    dBodySetRotation(body, rotationMatrix);

    dBodySetPosition(body,x,y,z);

    attachmentOffset = thickness + .25;

    color[0] = .5;
    color[1] = 1;
    color[2] = 1;
}

/**
 * This method initializes the grabbing of an object.
 * It creates an ODE-object with the current position and
 * orientation and stores the offset to the grabbed object
 * for correct update of the Entity-transformation
 * in the update method. It also creates all joints and
 * linked objects by calling the attachJoints-method.
 * @param entity Entity that should be grabbed
 **/
void JointInteraction::grabEntity(Entity* entity, TransformationData offset)
{
	unsigned entityID;
	unsigned typeBasedID;
	Entity* ent;
	dQuaternion quat;
	std::map< int, ODEObject*>::iterator it;
//	TransformationPipe* linkedObjectPipe;
//	unsigned short type, id;
	std::vector<unsigned> entityIds;
	unsigned userId;
	JointInteractionBeginEvent* evt;

	isGrabbing = true;

// // store position and orientation difference between user and object
	deltaTrans = offset;

// Create ODE-representation of grabbed object by simply creating an object with
// user's position and orientation
	ODEObject* odeObj = new ODEObject;
	odeObj->body = dBodyCreate(world);
	odeObj->entity = entity;

	dBodySetPosition(odeObj->body, userTrans.position[0], userTrans.position[1], userTrans.position[2]);
	convert(quat, userTrans.orientation);
	dBodySetQuaternion(odeObj->body, quat);

// attach all joints to object
	entityID = entity->getEnvironmentBasedId();
	linkedObjMap[entityID] = odeObj;
	attachJoints(entityID);

	grabbedObject = odeObj;

	entityIds.push_back(entity->getTypeBasedId());

// open Pipes for all linked objects
	for (it = linkedObjMap.begin(); it != linkedObjMap.end(); ++it)
	{
		if (it->second == grabbedObject)
			continue;
		ent = WorldDatabase::getEntityWithEnvironmentId(it->first);
		assert(ent->getEnvironmentBasedId() != entityID);
		typeBasedID = ent->getTypeBasedId();
		entityIds.push_back(typeBasedID);
/*
		split(typeBasedID, type, id);
// TODO: open pipes via Event
		linkedObjectPipe = TransformationManager::openPipe(JOINT_INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 0, 0, type, id, 0, false);
		linkedObjPipes[it->first] = linkedObjectPipe;
*/
	} // for

	userId = UserDatabase::getLocalUserId();
	evt = new JointInteractionBeginEvent(userId, entityIds);
	EventManager::sendEvent(evt, EventManager::EXECUTE_GLOBAL);

} // grabEntity

 void Primitive::setPosition(const Pos& pos){
   if(body){
     dBodySetPosition(body, pos.x(), pos.y(), pos.z());
   }else if(geom){ // okay there is just a geom no body
     dGeomSetPosition(geom, pos.x(), pos.y(), pos.z());
   }
   update(); // update the scenegraph stuff
 }

 void createPart(const dWorldID & _world , dPart & part, dReal * pos3){
     dMass mass;
     part.body = dBodyCreate(_world);
     dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position
     dMassSetZero(&mass);                        // Set mass parameter to zero
     dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length);  // Calculate mass parameter
     dBodySetMass(part.body, &mass);  // Set mass
 }

void ODERigidObject::SetTransform(const RigidTransform& T)
{
  Vector3 comPos = T*obj.com;
  dBodySetPosition(bodyID,comPos.x,comPos.y,comPos.z);
  dMatrix3 rot;
  CopyMatrix(rot,T.R);
  dBodySetRotation(bodyID,rot);
}

void ODE_Link::setTransform(const hrp::Vector3& pos, const hrp::Matrix33& R){
    hrp::Vector3 _pos(R * C + pos);
    dBodySetPosition(bodyId, _pos(0), _pos(1), _pos(2));
    dMatrix3 _R = {R(0,0), R(0,1), R(0,2), 0,
                  R(1,0), R(1,1), R(1,2), 0,
                  R(2,0), R(2,1), R(2,2), 0};
    dBodySetRotation(bodyId, _R);
}

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);
    }
  }
}

/**
 * This method returns the ODE-representation of the
 * Entity with the passed ID. It searches
 * in the map of connected objects for a matching
 * ODEObject. If no object is found the method
 * creates one and stores it in the map.
 * @param entityID ID of the EntityTransform
 * @return ODE representation of the EntityTransform
 **/
dBodyID JointInteraction::getBodyWithID(int entityID)
{
	if (entityID == 0)
		return 0;

// 	TransformationPipe* objectPipe;
	Entity* entity;
	TransformationData trans;
	gmtl::Vec3f pos;
	gmtl::Quatf ori;
	gmtl::AxisAnglef axAng;
	dQuaternion quat;

	ODEObject* object = linkedObjMap[entityID];
	if (object == NULL)
	{
		entity = getEntityWithID(entityID);
		if (entity == NULL)
		{
			printd(ERROR, "JointInteraction::getBodyWithID(): ERROR: FOUND BODY WITHOUT MATCHING ENTITY OBJECT!!!\n");
			return 0;
		} // if

		object = new ODEObject;
		object->body = dBodyCreate(world);
		object->entity = entity;

		trans = entity->getEnvironmentTransformation();
		pos = trans.position;
		ori = trans.orientation;
	// get current object position and orientation
// 		pos[0] = entity->getXTrans();
// 		pos[1] = entity->getYTrans();
// 		pos[2] = entity->getZTrans();
// 		axAng.set(entity->getRotAngle(), entity->getXRot(), entity->getYRot(), entity->getZRot());
// 		gmtl::set(ori, axAng);

		convert(quat, ori);
		dBodySetPosition(object->body, pos[0], pos[1], pos[2]);
		dBodySetQuaternion(object->body, quat);

		linkedObjMap[entityID] = object;

// TODO: open and close pipes via events
// open TransformationPipe for handling transformations of connected entities
// 		split(entity->getTypeBasedId(), type, id);
// 		objectPipe = TransformationManager::openPipe(JOINT_INTERACTION_MODULE_ID, WORLD_DATABASE_ID, 0, 0, type, id, 0, 0);
// 		linkedObjPipes[entityID] = objectPipe;

// ingnore request if Entity is not movable
		if (!entity->getEntityType()->isFixed())
			attachJoints(entityID);
	} // if
	if (object->entity->getEntityType()->isFixed())
		return 0;

	return object->body;
} // getBodyWithID

/*** ロボットアームの生成 ***/
void  makeArm()
{
  dMass mass;                                    // 質量パラメータ
  dReal x[NUM]      = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 重心 x
  dReal y[NUM]      = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 重心 y
  dReal z[NUM]      = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0};  // 重心 z
  dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50};  // 長さ
  dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50};  // 質量
  dReal r[NUM-1]      = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};  // 半径
  dReal c_x[NUM]    = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 関節中心点 x
  dReal c_y[NUM]    = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 関節中心点 y
  dReal c_z[NUM]    = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9};  // 関節中心点 z
  dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0};              // 関節回転軸 x
  dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0};              // 関節回転軸 y
  dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1};              // 関節回転軸 z

  // リンクの生成
  for (int i = 0; i < NUM-1; i++) {
    rlink[i].body = dBodyCreate(world);
    dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
    dMassSetZero(&mass);
    dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
    dBodySetMass(rlink[i].body, &mass);
    rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
    dGeomSetBody(rlink[i].geom,rlink[i].body);
  }
  rlink[NUM-1].body = dBodyCreate(world);
  dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
  dBodySetMass(rlink[NUM-1].body, &mass);
  rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
  dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);

  // ジョイントの生成とリンクへの取り付け
  joint[0] = dJointCreateFixed(world, 0);  // 固定ジョイント
  dJointAttach(joint[0], rlink[0].body, 0);
  dJointSetFixed(joint[0]);
  for (int j = 1; j < NUM; j++) {
    joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
    dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
    dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
    dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
  }
}

void TSRODERigidBody::SetBodyTransform( const TSRMatrix4& _bodyTransform )
{
    dMatrix4 R;
    dVector3 P;
    Matrix4ToODE( _bodyTransform, R, P );
    dBodySetPosition( m_BodyID, P[ 0 ], P[ 1 ], P[ 2 ] );
    dBodySetRotation( m_BodyID, R );

}

/* sets position and rotation of a body based on position and quaternion values */
void body_set_position_and_quaternion (dBodyID b, t_real x, t_real y, t_real z, t_real q1, t_real q2, t_real q3, t_real q4) {
  dQuaternion q;
  dBodySetPosition (b, x, y, z);
  q [0] = q1;
  q [1] = q2;
  q [2] = q3;
  q [3] = q4;
  dBodySetQuaternion (b, q);
}

static void reset_state(void)
{
  float sx=-4, sy=-4, sz=2;
  dQuaternion q;
  dQFromAxisAndAngle (q,1,0,0,M_PI*0.5);
#ifdef BOX
  dBodySetPosition (boxbody, sx, sy+1, sz);
  dBodySetLinearVel (boxbody, 0,0,0);
  dBodySetAngularVel (boxbody, 0,0,0);
  dBodySetQuaternion (boxbody, q);
#endif
#ifdef CYL
  dBodySetPosition (cylbody, sx, sy, sz);
  dBodySetLinearVel (cylbody, 0,0,0);
  dBodySetAngularVel (cylbody, 0,0,0);
  dBodySetQuaternion (cylbody, q);
#endif
}

void MyDeformableObjectNode::updatePhysicsBodies(int vertexIndex) {
    if (mPrintDebugOutput) {
        std::cout << mDebugOutputPrefix << "MyDeformableObjectNode::updatePhysicsBodies(vertexIndex: " << vertexIndex << ")" << std::endl;
    }

    if (!mPhysicsObjects.empty()) {

        unsigned int index, objectCount;

        dcollide::Vertex* vertex;
        MyODEDeformableTestAppGeom* geom;


        if (vertexIndex != -1) {
            index = vertexIndex;
            objectCount = vertexIndex+1;
        } else {
            index = 0;
            objectCount = mPhysicsObjects.size();
        }

        const std::vector<dcollide::Vertex*>& vertices
            = getProxy()->getShape()->getMesh()->getVertices();
        for (/* nop */; index < objectCount; ++index) {

            geom = mPhysicsObjects[index];
            vertex = vertices[index];

            dcollide::Vector3 bodyPosition = vertex->getWorldPosition();

            if (mPrintDebugOutput) {
                if (index == objectCount-1) {
                    const dReal* odePosition = 0;
                    odePosition = dBodyGetPosition(geom->getBody());

                    std::cout << mDebugOutputPrefix << "----------" << std::endl;
                    std::cout << mDebugOutputPrefix << "ODE Position (before move): " << odePosition[0] << ", " << odePosition[1] << ", " << odePosition[2] << std::endl;
                    std::cout << mDebugOutputPrefix << "D-Collide World Position: " << bodyPosition << std::endl << std::endl;
                }
            }

            geom->setMoveNodeOnBodyMoved(false);
            dBodySetPosition(geom->getBody(),  bodyPosition.getX(),
                                               bodyPosition.getY(),
                                               bodyPosition.getZ());
            geom->setMoveNodeOnBodyMoved(true);

            if (mPrintDebugOutput) {
                if (index == objectCount-1) {
                    const dReal* odePosition = 0;
                    odePosition = dBodyGetPosition(geom->getBody());
                    std::cout << mDebugOutputPrefix << "ODE Position (after move): " << odePosition[0] << ", " << odePosition[1] << ", " << odePosition[2] << std::endl << std::endl;
                }
            }
        }
    }
}

void CubeBasePiece::attachToBase(dBodyID otherBody, dWorldID world, dJointGroupID jointGroup, dReal x, dReal y, dReal z, const dMatrix3 rotationMatrix)
{
    dBodySetPosition(body,x,y,z);

    dJointID connectingJoint = dJointCreateSlider(world,jointGroup);
    dJointAttach(connectingJoint,otherBody,body);
    dJointSetSliderParam(connectingJoint,dParamLoStop,0);
    dJointSetSliderParam(connectingJoint,dParamHiStop,0);
}

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;

  // create world
  dInitODE2(0);
  world = dWorldCreate();

  dMass m;
  dMassSetBox (&m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&m,MASS);

  dQuaternion q;
  dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);

  body[0] = dBodyCreate (world);
  dBodySetMass (body[0],&m);
  dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
  dBodySetQuaternion (body[0],q);

  body[1] = dBodyCreate (world);
  dBodySetMass (body[1],&m);
  dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
  dBodySetQuaternion (body[1],q);

  hinge = dJointCreateHinge (world,0);
  dJointAttach (hinge,body[0],body[1]);
  dJointSetHingeAnchor (hinge,0,0,1);
  dJointSetHingeAxis (hinge,1,-1,1.41421356);

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dWorldDestroy (world);
  dCloseODE();
  return 0;
}

IoObject *IoODEBody_setPosition(IoODEBody *self, IoObject *locals, IoMessage *m)
{
	const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
	const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
	const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);

	IoODEBody_assertValidBody(self, locals, m);
	dBodySetPosition(BODYID, x, y, z);
	return self;
}

void ode::newLine(float xp, float yp)
{
		b = dBodyCreate (world);
		dBodySetPosition (b,xp,yp,2);
		//dMassSetSphere (&m,1,RADIUS);
		dMassAdjust (&m, 1);
		dBodySetMass (b,&m);
		lineStrips[lines] = dCreateSphere (space,RADIUS);
		dGeomSetBody (lineStrips[lines++],b);		
}

void SkidSteeringVehicle::create() {
    this->vehicleBody = dBodyCreate(this->environment->world);
    this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
    this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
    dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
    dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
    dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
    dBodySetMass(this->vehicleBody, &this->vehicleMass);
    dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
    dGeomSetBody(this->vehicleGeom, this->vehicleBody);
    dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);

    dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
            this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
            dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
            dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
            dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
            this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
            dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
            dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
            dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
            dMatrix3 wheelR;
            dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
            dBodySetRotation(this->wheelBody[fr][lr], wheelR);
            this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
            dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
            dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
            dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
            dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
        }
    }
    
    this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
        }
    }
}

static void reset_ball(void)
{
  float sx=0.0f, sy=3.40f, sz=7.15;

  dQuaternion q;
  dQSetIdentity(q);
  dBodySetPosition (sphbody, sx, sy, sz);
  dBodySetQuaternion(sphbody, q);
  dBodySetLinearVel (sphbody, 0,0,0);
  dBodySetAngularVel (sphbody, 0,0,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);
}