C++ dBodySetRotation函数代码示例

void dGeomSetRotation (dxGeom *g, const dMatrix3 R)
{
    dAASSERT (g && R);
    dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
    CHECK_NOT_LOCKED (g->parent_space);
    if (g->offset_posr) {
        g->recomputePosr();
        // move body such that body+offset = rotation
        dxPosR new_final_posr;
        dxPosR new_body_posr;
        memcpy(new_final_posr.pos, g->final_posr->pos, sizeof(dVector3));
        memcpy(new_final_posr.R, R, sizeof(dMatrix3));
        getBodyPosr(*g->offset_posr, new_final_posr, new_body_posr);
        dBodySetRotation(g->body, new_body_posr.R);
        dBodySetPosition(g->body, new_body_posr.pos[0], new_body_posr.pos[1], new_body_posr.pos[2]);
    }
    else if (g->body) {
        // this will call dGeomMoved (g), so we don't have to
        dBodySetRotation (g->body,R);
    }
    else {
        memcpy (g->final_posr->R,R,sizeof(dMatrix3));
        dGeomMoved (g);
    }
}

void place_cards()
{
    ncards = getncards(levels);
    // destroy removed cards (if any)
    int oldcards = cards.size();
    for (int i=ncards; i<oldcards; ++i)
        delete cards[i];
    cards.resize(ncards);
    // construct new cards (if any)
    for (int i=oldcards; i<ncards; ++i)
        cards[i] = new Card;
    
    // for each level
    int c = 0;
    dMatrix3 right, left, hrot;
    dReal angle = 20*M_PI/180.;
    dRFromAxisAndAngle(right, 1, 0, 0, -angle);
    dRFromAxisAndAngle(left, 1, 0, 0, angle);

    dRFromAxisAndAngle(hrot, 1, 0, 0, 91*M_PI/180.);
    
    dReal eps = 0.05;
    dReal vstep = cos(angle)*clength + eps;
    dReal hstep = sin(angle)*clength + eps;
    
    for (int lvl=0; lvl<levels; ++lvl) {
        // there are 3*(levels-lvl)-1 cards in each level, except last
        int n = (levels-lvl);
        dReal height = (lvl)*vstep + vstep/2;
        // inclined cards
        for (int i=0; i<2*n; ++i, ++c) {
            dBodySetPosition(cards[c]->body, 
                    0,
                    -n*hstep + hstep*i,
                    height
                    );
            if (i%2)
                dBodySetRotation(cards[c]->body, left);
            else
                dBodySetRotation(cards[c]->body, right);
        }
        
        if (n==1) // top of the house
            break;
        
        // horizontal cards
        for (int i=0; i<n-1; ++i, ++c) {
            dBodySetPosition(cards[c]->body,
                    0,
                    -(n-1 - (clength-hstep)/2)*hstep + 2*hstep*i,
                    height + vstep/2);
            dBodySetRotation(cards[c]->body, hrot);
        }
    }
    
}

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

CubeBasePiece::CubeBasePiece(dWorldID& world,dSpaceID& space, float x, float y, float z)
{
    body = dBodyCreate(world);
    geom = dCreateBox(space, sides[0], sides[1], sides[2]);
    dGeomSetBody(geom, body);
    dGeomSetData(geom, this);
    dMass mass;
    mass.setBox(CUBE_PIECE_DENSITY, sides[0], sides[1], sides[2]);
    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);

    for(int i = 0 ; i < 6 ; i++)
        attachedPieces[i] = NULL; // initialize attached piece array to all null pointers

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

void set_phys_rotation(dBodyID body, const float *r)
{
    dMatrix3 R;

    set_rotation(R, r);
    dBodySetRotation(body, R);
}

//===========================================================================
void cODEGenericBody::setRotation(cMatrix3d &a_rotation)
{
    // apply new rotation to ODE body
	dMatrix3 R;

	R[0]  = a_rotation.m[0][0];
	R[1]  = a_rotation.m[0][1];
	R[2]  = a_rotation.m[0][2];
	R[4]  = a_rotation.m[1][0];
	R[5]  = a_rotation.m[1][1];
	R[6]  = a_rotation.m[1][2];
	R[8]  = a_rotation.m[2][0];
	R[9]  = a_rotation.m[2][1];
	R[10] = a_rotation.m[2][2];

    // check if body defined
    if (m_ode_body != NULL)
    {
        // store new rotation matrix
        m_localRot = a_rotation;
        dBodySetRotation(m_ode_body, R);
    }
    else if (m_ode_geom != NULL)
    {
        // store new rotation matrix
        m_localRot = a_rotation;
        dGeomSetRotation(m_ode_geom, R);
    }
}

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

void CProtoHapticDoc::UpdateDynamics()
{
	for(int i= 0; i<m_shapeCount; i++) {

		dGeomBoxSetLengths (m_geoms[i], m_shapes[i]->getSizeX(),
									    m_shapes[i]->getSizeY(),
									    m_shapes[i]->getSizeZ());

		dGeomSetPosition   (m_geoms[i], m_shapes[i]->getLocationX(),
									    m_shapes[i]->getLocationY(),
									    m_shapes[i]->getLocationZ());
		dGeomSetRotation   (m_geoms[i], dBodyGetRotation(bodies[i]));

		dBodySetPosition   (bodies[i], m_shapes[i]->getLocationX(),
									   m_shapes[i]->getLocationY(),
									   m_shapes[i]->getLocationZ());

		float *rotation= m_shapes[i]->getRotation();

		const dReal rot[12]=
		{ rotation[0], rotation[4], rotation[8], rotation[12],
		  rotation[1], rotation[5], rotation[9], rotation[13],
		  rotation[2], rotation[6], rotation[10], rotation[14] };

		dBodySetRotation   (bodies[i], rot);

		dMass mass;
		dMassSetBox (&mass, m_shapes[i]->getMass(),m_shapes[i]->getSizeX(),
								                   m_shapes[i]->getSizeY(),
								                   m_shapes[i]->getSizeZ());

		dBodySetMass (bodies[i], &mass);

	}
}

    void cPhysicsObject::InitCommon(cWorld* pWorld, const physvec_t& posOriginal, const physvec_t& rot)
    {
      math::cVec3 pos(posOriginal.x, posOriginal.y, posOriginal.z + fHeight);

      rotation.LoadIdentity();
      rotation.SetFromAngles(math::DegreesToRadians(rot));

      const math::cMat4 m = rotation.GetMatrix();

      dMatrix3 r;
      r[0] = m[0];    r[1] = m[4];    r[2] = m[8];    r[3] = 0;
      r[4] = m[1];    r[5] = m[5];    r[6] = m[9];    r[7] = 0;
      r[8] = m[2];    r[9] = m[6];    r[10] = m[10];  r[11] = 0;

      position = pos;

      dGeomSetPosition(geom, position.x, position.y, position.z);
      dGeomSetRotation(geom, r);

      if (bBody) {
        body = dBodyCreate(pWorld->GetWorld());
        dBodySetPosition(body, position.x, position.y, position.z);
        dBodySetRotation(body, r);
        dBodySetAutoDisableFlag(body, 1);

        dGeomSetBody(geom, body);

        pWorld->AddPhysicsObject(shared_from_this());
      }
    }

/*
=================================================================================
createFixedLeg

	Use parameters to create leg body/geom and attach to body with fixed joint
=================================================================================
*/
void createFixedLeg(ODEObject &leg,
	ODEObject &bodyAttachedTo,
	dJointID& joint,
	dReal xPos, dReal yPos, dReal zPos,
	dReal xRot, dReal yRot, dReal zRot,
	dReal radius,
	dReal length)
{
	dMatrix3 legOrient;
	dRFromEulerAngles(legOrient, xRot, yRot, zRot);

	//position and orientation
	leg.Body = dBodyCreate(World);
	dBodySetPosition(leg.Body, xPos, yPos, zPos);
	dBodySetRotation(leg.Body, legOrient);
	dBodySetLinearVel(leg.Body, 0, 0, 0);
	dBodySetData(leg.Body, (void *)0);

	//mass
	dMass legMass;
	dMassSetCapsule(&legMass, 1, 3, radius, length);
	dBodySetMass(leg.Body, &legMass);

	//geometry
	leg.Geom = dCreateCapsule(Space, radius, length);
	dGeomSetBody(leg.Geom, leg.Body);

	//fixed joint
	joint = dJointCreateFixed(World, jointgroup);
	dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
	dJointSetFixed(joint);
}

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

/* sets the orientation axes of a body */
void body_set_axes (dBodyID b, const t_real *u, const t_real *v, const t_real *t) {
  dMatrix3 r;
  r [0] = u[0]; r [4] = u[1]; r [8]  = u[2];
  r [1] = v[0]; r [5] = v[1]; r [9]  = v[2];
  r [2] = t[0]; r [6] = t[1]; r [10] = t[2];
  r [3] = 0.;   r [7] = 0.;   r [11] = 0.;
  dBodySetRotation (b, r);
}

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

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

}