C++ dJointAttach函数代码示例

osaODEServoMotor::osaODEServoMotor(dWorldID world, 
				       dBodyID body1, 
				       dBodyID body2,
				       const vctFixedSizeVector<double,3>& axis,
				       double vwmax,
				       double ftmax,
				       dJointType motortype ) : 
  vwmax( fabs( vwmax ) ),
  ftmax( fabs( ftmax ) ) {
  
  if( motortype == dJointTypeHinge ){
    motorid = dJointCreateAMotor( world, 0 );       // create the motor
    dJointAttach( MotorID(), body1, body2 );        // attach the joint
    dJointSetAMotorMode( MotorID(), dAMotorUser );  // motor is in user mode
    dJointSetAMotorNumAxes( MotorID(), 1 );         // only 1 axis
    dJointSetAMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );

    SetVelocity( 0.0 );    // idle the motor
  }

  if( motortype == dJointTypeSlider ){
    motorid = dJointCreateLMotor( world, 0 );     // create the motor
    dJointAttach( MotorID(), body1, body2 );      // attach the joint
    dJointSetLMotorNumAxes( MotorID(), 1 );       // 1 axis 
    dJointSetLMotorAxis( MotorID(), 0, 2, axis[0], axis[1], axis[2] );

    SetVelocity( 0.0 );    // idle the motor
  }
  
}

// ang2 = position angle
// ang  = rotation angle
Robot::Wheel::Wheel(Robot* robot,int _id,float ang,float ang2,int wheeltexid)
{
    id = _id;
    rob = robot;
    float rad = rob->cfg->robotSettings.RobotRadius - rob->cfg->robotSettings.WheelThickness / 2.0;
    ang *= M_PI/180.0f;
    ang2 *= M_PI/180.0f;
    float x = rob->m_x;
    float y = rob->m_y;
    float z = rob->m_z;
    float centerx = x+rad*cos(ang2);
    float centery = y+rad*sin(ang2);
    float centerz = z-rob->cfg->robotSettings.RobotHeight*0.5+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight;
    cyl = new PCylinder(centerx,centery,centerz,rob->cfg->robotSettings.WheelRadius,rob->cfg->robotSettings.WheelThickness,rob->cfg->robotSettings.WheelMass,0.9,0.9,0.9,wheeltexid);
    cyl->setRotation(-sin(ang),cos(ang),0,M_PI*0.5);
    cyl->setBodyRotation(-sin(ang),cos(ang),0,M_PI*0.5,true);       //set local rotation matrix
    cyl->setBodyPosition(centerx-x,centery-y,centerz-z,true);       //set local position vector
    cyl->space = rob->space;

    rob->w->addObject(cyl);

    joint = dJointCreateHinge (rob->w->world,0);

    dJointAttach (joint,rob->chassis->body,cyl->body);
    const dReal *a = dBodyGetPosition (cyl->body);
    dJointSetHingeAxis (joint,cos(ang),sin(ang),0);
    dJointSetHingeAnchor (joint,a[0],a[1],a[2]);

    motor = dJointCreateAMotor(rob->w->world,0);
    dJointAttach(motor,rob->chassis->body,cyl->body);
    dJointSetAMotorNumAxes(motor,1);
    dJointSetAMotorAxis(motor,0,1,cos(ang),sin(ang),0);
    dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax);
    speed = 0;
}

void TrackedVehicle::create() {
    this->vehicleBody = dBodyCreate(this->environment->world);
    this->vehicleGeom = dCreateBox(this->environment->space, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
    this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
    dMassSetBox(&this->vehicleMass, this->density, this->leftTrack->m->distance, this->width, this->leftTrack->m->radius[0]);
    //dMassAdjust(&this->vehicleMass, 2.40);
    dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
    dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
    dBodySetMass(this->vehicleBody, &this->vehicleMass);
    dGeomSetBody(this->vehicleGeom, this->vehicleBody);
    dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->leftTrack->m->radius[0]);

    this->leftTrack->create();
    this->rightTrack->create();

    dReal w = this->width + 2*trackWidth + 2 * trackVehicleSpace;
    dRigidBodyArraySetPosition(leftTrack->bodyArray,  -wheelBase/2, -(w - trackWidth)/2, 0);
    dRigidBodyArraySetPosition(rightTrack->bodyArray, -wheelBase/2,  (w - trackWidth)/2, 0);

    this->leftTrackJoint = dJointCreateFixed(this->environment->world, 0);
    this->rightTrackJoint = dJointCreateFixed(this->environment->world, 0);
    dJointAttach(this->leftTrackJoint, this->vehicleBody, this->leftTrack->trackBody);
    dJointAttach(this->rightTrackJoint, this->vehicleBody, this->rightTrack->trackBody);
    dJointSetFixed(this->leftTrackJoint);
    dJointSetFixed(this->rightTrackJoint);

    this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
    dRigidBodyArrayAdd(this->bodyArray, this->leftTrack->bodyArray);
    dRigidBodyArrayAdd(this->bodyArray, this->rightTrack->bodyArray);
}

Rope::Rope(Object* obja, Body* bodya, Object* objb, Body* bodyb)
    : obja_(obja), objb_(objb), selected_(false)
{
    vec apos = bodya->position();
    proxya_.set_position(apos);
    proxya_.set_velocity(bodya->velocity());
    proxya_.set_mass(0.01, 1);
    hingea_ = dJointCreateHinge(LEVEL->world, 0);
    dJointAttach(hingea_, proxya_.body_id(), bodya->body_id());
    dJointSetHingeAxis(hingea_, 0, 0, 1);

    vec bpos = bodyb->position();
    proxyb_.set_position(bpos);
    proxyb_.set_velocity(bodyb->velocity());
    proxyb_.set_mass(0.01, 1);
    hingeb_ = dJointCreateHinge(LEVEL->world, 0);
    dJointAttach(hingeb_, proxyb_.body_id(), bodyb->body_id());
    dJointSetHingeAxis(hingeb_, 0, 0, 1);
    
    rope_ = dJointCreateSlider(LEVEL->world, 0);
    vec axis = bpos - apos;
    dJointAttach(rope_, proxya_.body_id(), proxyb_.body_id());
    dJointSetSliderAxis(rope_, axis.x, axis.y, 0);
    
    dJointSetSliderParam(rope_, dParamLoStop, 0);
    dJointSetSliderParam(rope_, dParamStopCFM, 0.25);
    dJointSetSliderParam(rope_, dParamStopERP, 0.01);

    ext_ = base_ext_ = axis.norm();
}

void CProtoHapticDoc::nearCallback(dGeomID o1, dGeomID o2)
{
	int index1= (int)dGeomGetData(o1);
	int index2= (int)dGeomGetData(o2);

	if(m_shapes[index1]->isCollisionDynamic() || 
	   m_shapes[index2]->isCollisionDynamic()) {

		int n, i;
		const int N = 50;
		dContact contact[N];
		n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
		if (n > 0) {
			OutputDebugString("Collision\n");
			for (i=0; i<n; i++) {
				contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactSoftERP | dContactSoftCFM | dContactApprox1;
				if (dGeomGetClass(o1) == dSphereClass || dGeomGetClass(o2) == dSphereClass)
					contact[i].surface.mu = 20;
				else
					contact[i].surface.mu = 0.5;
				contact[i].surface.slip1 = 1.0 - (m_shapes[index1]->getSurfaceFriction());
				contact[i].surface.slip2 = 1.0 - (m_shapes[index2]->getSurfaceFriction());
				contact[i].surface.soft_erp = 0.8;
				contact[i].surface.soft_cfm = 0.01;
				dJointID c = dJointCreateContact (m_worldID,m_jointGroup,contact+i);
				dJointAttach (c,dGeomGetBody(o1),dGeomGetBody(o2));
			}
		}
	}

}

void createInvisibleHead( float* pos )
{
	dMatrix3 head_orientation;
	dRFromEulerAngles(head_orientation, 0.0, 0.0, 0.0);

	//position and orientation
	head.Body = dBodyCreate(World);
	dBodySetPosition(head.Body, pos[ 0 ], pos[ 1 ], pos[ 2 ]);
	dBodySetRotation(head.Body, head_orientation);
	dBodySetLinearVel(head.Body, 0, 0, 0);
	dBodySetData(head.Body, (void *)0);

	//mass
	dMass head_mass;
	dMassSetBox(&head_mass, 1.0, 1.0, 1.0, 1.0);
	dBodySetMass(head.Body, &head_mass);

	//geometry
	head.Geom = dCreateBox(Space, 1.0, 1.0, 1.0);
	dGeomSetBody(head.Geom, head.Body);

	//fixed joint
	invis_box_joint = dJointCreateFixed(World, jointgroup);
	dJointAttach(invis_box_joint, body.Body, head.Body);
	dJointSetFixed(invis_box_joint);
}

// Create a ball and a pole
void createBallandPole() {
	dMass m1;
	dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;

	// ball
	ball.radius = 0.2;
	ball.mass = 1.0;
	ball.body = dBodyCreate(world);
	dMassSetZero(&m1);
	dMassSetSphereTotal(&m1, ball.mass, ball.radius);
	dBodySetMass(ball.body, &m1);
	dBodySetPosition(ball.body, x0, y0, z0);

	ball.geom = dCreateSphere(space, ball.radius);
	dGeomSetBody(ball.geom, ball.body);

	// pole
	pole.radius = 0.025;
	pole.length = 1.0;
	pole.mass = 1.0;
	pole.body = dBodyCreate(world);
	dMassSetZero(&m1);
	dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
	dBodySetMass(pole.body, &m1);
	dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);

	pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
	dGeomSetBody(pole.geom, pole.body);

	// hinge joint
	joint = dJointCreateHinge(world, 0);
	dJointAttach(joint, ball.body, pole.body);
	dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
	dJointSetHingeAxis(joint, 1, 0, 0);
}

void Robots::construirChassi(dWorldID world)
{
    for (int i=0; i < 2; i++)
    {
        // Cria objeto e geometria
        this->body[i] = dBodyCreate(world);
        this->box[i] = dCreateBox(0,LENGTH/(1+i),WIDTH,HEIGHT);

        // Define a posição do objeto
        dBodySetPosition(this->body[i],this->pegarX(),this->pegarY(),STARTZ+HEIGHT/2-HEIGHT*i);

		// Se o robô for do segundo time, deve ser rotacionado em 180 graus
        if ((this->id == 3) || (this->id == 4) || (this->id == 5))
        {
			dQuaternion q;
			dQFromAxisAndAngle(q,0,0,1,M_PI);
			dBodySetQuaternion(this->body[i],q);
		}

        // Define a massa do objeto
        dMass m;
        dMassSetBox(&m,1,LENGTH/(1+i),WIDTH,HEIGHT);	// O segundo bloco é mais curto
        dMassAdjust(&m,CMASS*(1+i*2));					// O segundo bloco é mais pesado
        dBodySetMass(this->body[i],&m);
                
        // Associa o objeto à sua geometria
        dGeomSetBody(this->box[i],this->body[i]);
    }
		
    // O chassis é composto por dois blocos que são fixos entre si
    dJointID fixed = dJointCreateFixed(world,0);
    dJointAttach(fixed,this->body[1],this->body[0]);
    dJointSetFixed(fixed); 
}

static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
  assert(o1);
  assert(o2);

  if (dGeomIsSpace(o1) || dGeomIsSpace(o2))
  {
    fprintf(stderr,"testing space %p %p\n", o1,o2);
    // colliding a space with something
    dSpaceCollide2(o1,o2,data,&nearCallback);
    // Note we do not want to test intersections within a space,
    // only between spaces.
    return;
  }

  const int N = 32;
  dContact contact[N];
  int n = dCollide (o1,o2,N,&(contact[0].geom),sizeof(dContact));
  if (n > 0) 
  {
    for (int i=0; i<n; i++) 
    {
      contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
      contact[i].surface.mu = 100.0;
      contact[i].surface.soft_erp = 0.96;
      contact[i].surface.soft_cfm = 0.02;
      dJointID c = dJointCreateContact (world,contactgroup,&contact[i]);
      dJointAttach (c,
		    dGeomGetBody(contact[i].geom.g1),
		    dGeomGetBody(contact[i].geom.g2));
    }
  }
}

void Spikey::stick(Body* b) {
    if (hinge_) dJointDestroy(hinge_);
    hinge_ = dJointCreateFixed(LEVEL->world, 0);
    dJointAttach(hinge_, body_.body_id(), b ? b->body_id() : 0);
    dJointSetFixed(hinge_);
    state_ = STUCK;
}

void PhysicsWorld::collide(void *_data, dGeomID _o1, dGeomID _o2)
{
  int i;
  //std::cout<<"collide\n";
  // exit without doing anything if the two bodies are connected by a joint
  dBodyID b1 = dGeomGetBody(_o1);
  dBodyID b2 = dGeomGetBody(_o2);
  if (b1 && b2 && dAreConnectedExcluding (b1,b2,dJointTypeContact)) return;

  for (i=0; i<m_maxContacts; i++)
  {
    m_contact[i].surface.mode = dContactBounce;// | dContactSoftCFM;
    m_contact[i].surface.mu = dInfinity;
    m_contact[i].surface.mu2 = 0;
    m_contact[i].surface.bounce = 0.3;
    m_contact[i].surface.bounce_vel = 0.1;
    m_contact[i].surface.soft_cfm = 0.01;
  }
  if (int numc = dCollide (_o1,_o2,m_maxContacts,&m_contact[0].geom,sizeof(dContact)))
  {
    dMatrix3 RI;
    dRSetIdentity (RI);
    const dReal ss[3] = {0.02,0.02,0.02};
    for (i=0; i<numc; i++)
    {
      dJointID c = dJointCreateContact (m_world,m_contactgroup,m_contact+i);
      dJointAttach (c,b1,b2);
    }
  }
}

Robot::Kicker::Kicker(Robot* robot)
{
    rob = robot;

    float x = rob->m_x;
    float y = rob->m_y;
    float z = rob->m_z;
    float centerx = x+(rob->cfg->robotSettings.RobotCenterFromKicker+rob->cfg->robotSettings.KickerThickness);
    float centery = y;
    float centerz = z-(rob->cfg->robotSettings.RobotHeight)*0.5f+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight+rob->cfg->robotSettings.KickerZ;
    box = new PBox(centerx,centery,centerz,rob->cfg->robotSettings.KickerThickness,rob->cfg->robotSettings.KickerWidth,rob->cfg->robotSettings.KickerHeight,rob->cfg->robotSettings.KickerMass,0.9,0.9,0.9);
    box->setBodyPosition(centerx-x,centery-y,centerz-z,true);
    box->space = rob->space;

    rob->w->addObject(box);

    joint = dJointCreateHinge (rob->w->world,0);
    dJointAttach (joint,rob->chassis->body,box->body);
    const dReal *aa = dBodyGetPosition (box->body);
    dJointSetHingeAnchor (joint,aa[0],aa[1],aa[2]);
    dJointSetHingeAxis (joint,0,-1,0);

    dJointSetHingeParam (joint,dParamVel,0);
    dJointSetHingeParam (joint,dParamLoStop,0);
    dJointSetHingeParam (joint,dParamHiStop,0);

    rolling = 0;
    kicking = false;
}

void nearCallback(void *data, dGeomID o1, dGeomID o2) {
    State* state = (State*)data;

    if(dGeomIsSpace(o1) || dGeomIsSpace(o2)) {
        dSpaceCollide2(o1, o2, data, &nearCallback);

        if(dGeomIsSpace(o1))
            dSpaceCollide((dSpaceID)o1, data, &nearCallback);
        if(dGeomIsSpace(o2))
            dSpaceCollide((dSpaceID)o2, data, &nearCallback);
    } else {
        dBodyID b1 = dGeomGetBody(o1);
        dBodyID b2 = dGeomGetBody(o2);

        const int MAX_CONTACTS = 18;
        dContact contact[MAX_CONTACTS];

        for(int i = 0; i < MAX_CONTACTS; i++) {
            contact[i].surface.mode = dContactBounce;
            contact[i].surface.mu = 2000;
            contact[i].surface.bounce = 0.1;
            contact[i].surface.bounce_vel = 0.15;
        }

        if(int numc = dCollide(o1, o2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact))) {
            for(int i = 0; i < numc; i++) {
                dJointID c = dJointCreateContact(state->world, state->physicsContactgroup, &contact[i]);
                dJointAttach(c, b1, b2);
            }
        }
    }
}

//description
//Default behavior. 
//Default collision of bodies
void ODEBaseScene::Collide(dGeomID g1, dGeomID g2)
{
    int n;
    
    dBodyID b1 = dGeomGetBody(g1);
    dBodyID b2 = dGeomGetBody(g2);
    if (b1 && b2 && dAreConnected(b1, b2))
            return;
 
    const int N = 4;
    dContact contact[N];
    n = dCollide (g1,g2,N,&contact[0].geom,sizeof(dContact));
    if (n > 0) 
    {
        // printf("Body %d hits body %d.\n", (size_t) dGeomGetData(g1),(size_t) dGeomGetData(g2));
        for (int i=0; i<n; ++i)
        {
            // contact[i].surface.mode = dContactBounce | dContactSoftCFM;
            contact[i].surface.mode = dContactBounce;        
            //contact[i].surface.mu = dInfinity;
            contact[i].surface.mu = 0.5;
            //contact[i].surface.mu2 = 0.5;
            contact[i].surface.bounce = 0.000999990;
            //contact[i].surface.bounce_vel = 0.1;
            //contact[i].surface.soft_cfm = 0.001;

            //contact[i].surface.mode = dContactBounce|dContactSoftERP|dContactSoftCFM;
            contact[i].surface.soft_erp = 1.0; //1.0;
            contact[i].surface.soft_cfm = 1e-10;
            dJointID j = dJointCreateContact (m_domain->getWorld(), m_domain->getContactGroup(), contact+i);
            dJointAttach(j, b1, b2);
        }
    }
}

void	StaticEnvironment ( bool& do_colide, bool bo1, dContact& c, SGameMtl* material_1, SGameMtl* material_2 )
{
	dJointID contact_joint	= dJointCreateContact(0, ContactGroup, &c);

	if(bo1)
	{
		((CPHActivationShape*)(retrieveGeomUserData(c.geom.g1)->callback_data))->DActiveIsland()->ConnectJoint(contact_joint);
		dJointAttach			(contact_joint, dGeomGetBody(c.geom.g1), 0);
	}
	else
	{
		((CPHActivationShape*)(retrieveGeomUserData(c.geom.g2)->callback_data))->DActiveIsland()->ConnectJoint(contact_joint);
		dJointAttach			(contact_joint, 0, dGeomGetBody(c.geom.g2));
	}
	do_colide=false;
}