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;
}
// simulation loop
static void simLoop (int pause)
{
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
//@a sphere
dsSetColor(1,0,0); // set color (red, green, blue�j a value is between 0 and 1
dsSetSphereQuality(3); // set sphere quality. 3 is enough
pos1 = dBodyGetPosition(sphere.body); // get a body position
R1 = dBodyGetRotation(sphere.body); // get a body rotation matrix
dsDrawSphere(pos1,R1,radius); // draw a sphere
// a cylinder
dsSetColorAlpha (0,1,0,1);
pos2 = dBodyGetPosition(cylinder.body);
R2 = dBodyGetRotation(cylinder.body);
dsDrawCylinder(pos2,R2,length,radius); // draw a cylinder
// a capsule
dsSetColorAlpha (1,1,1,1);
pos2 = dBodyGetPosition(capsule.body);
R2 = dBodyGetRotation(capsule.body);
dsDrawCapsule(pos2,R2,length,radius); // draw a capsule
// a box
dsSetColorAlpha (0,0,1,1);
pos3 = dBodyGetPosition(box.body);
R3 = dBodyGetRotation(box.body);
dsDrawBox(pos3,R3,sides); // draw a box
// a ray
dReal posA[3] = {0, 5, 0}, posB[3]= {0, 5, 1.9};
dsDrawLine(posA,posB); // draw a ray
}
static void simLoop (int pause)
{
dSpaceCollide (space,0,&nearCallback);
if (!pause)
{
dWorldQuickStep (world, 0.01); // 100 Hz
}
dJointGroupEmpty (contactgroup);
dsSetColorAlpha (1,1,0,0.5);
const dReal *CPos = dBodyGetPosition(cylbody);
const dReal *CRot = dBodyGetRotation(cylbody);
float cpos[3] = {CPos[0], CPos[1], CPos[2]};
float crot[12] = { CRot[0], CRot[1], CRot[2], CRot[3], CRot[4], CRot[5], CRot[6], CRot[7], CRot[8], CRot[9], CRot[10], CRot[11] };
dsDrawCylinder
(
cpos,
crot,
CYLLENGTH,
CYLRADIUS
); // single precision
const dReal *SPos = dBodyGetPosition(sphbody);
const dReal *SRot = dBodyGetRotation(sphbody);
float spos[3] = {SPos[0], SPos[1], SPos[2]};
float srot[12] = { SRot[0], SRot[1], SRot[2], SRot[3], SRot[4], SRot[5], SRot[6], SRot[7], SRot[8], SRot[9], SRot[10], SRot[11] };
dsDrawSphere
(
spos,
srot,
SPHERERADIUS
); // single precision
}
//Fonction de dessin avec drawstuff
void dessin_env( Bloc *bloc1, Bloc *bloc2 ){
dReal sides[3] = {LARGEUR, LONGUEUR, EPAISSEUR };
dsSetColor (COULEUR_BLOC);
dsSetTexture (DS_WOOD);
dsDrawBox ( dBodyGetPosition( bloc1->bodyID ) , dBodyGetRotation( bloc1->bodyID ), sides);
dsDrawBox ( dBodyGetPosition( bloc2->bodyID ) , dBodyGetRotation( bloc2->bodyID ), sides);
}
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);
}
}
}
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);
}
void odeSpringRender() {
glEnable( GL_BLEND );
glBlendFunc( GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA );
glPointSize( 4.f );
for( int i=0; i<ODESPRINGS_MAX; i++ ) {
if( odeSprings[i].active ) {
if( odeSprings[i].disabled ) {
continue;
glColor4ub( 120, 120, 120, 128 );
}
else {
glColor4ub( 120, 120, 230, 255 );
}
FVec3 p0;
if( odeSprings[i].dBody[0] ) {
dReal *pos0 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[0] );
dReal *rot0 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[0] );
FMat4 mat0 = zmathODERotPosToFMat4( rot0, pos0 );
p0 = mat0.mul( odeSprings[i].offset[0] );
}
else {
p0 = odeSprings[i].offset[0];
}
FVec3 p1;
if( odeSprings[i].dBody[1] ) {
dReal *pos1 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[1] );
dReal *rot1 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[1] );
FMat4 mat1 = zmathODERotPosToFMat4( rot1, pos1 );
p1 = mat1.mul( odeSprings[i].offset[1] );
}
else {
p1 = odeSprings[i].offset[1];
}
glBegin( GL_LINES );
glVertex3fv( p0 );
glVertex3fv( p1 );
glEnd();
glBegin( GL_POINTS );
FVec3 hack = p1;
hack.sub( p0 );
hack.normalize();
hack.mul( odeSprings[i].naturalLen );
hack.add( p0 );
glVertex3fv( hack );
glEnd();
}
}
}
void doDraw(){
if (!leg) return;
dsSetColor(0.0,0.0,1.0);
dsDrawCapsuleD(dBodyGetPosition(leg->lower_part.body), dBodyGetRotation(leg->lower_part.body), leg->lower_part.length, leg->lower_part.radius);
dsSetColor(1.0,0.0,0.0);
dsDrawCapsuleD(dBodyGetPosition(leg->upper_part.body), dBodyGetRotation(leg->upper_part.body), leg->upper_part.length, leg->upper_part.radius);
//TEST
double k1 = 1.0, fMax = 10.0; // k1: proportional gain, fMax：Max torque[Nm]
//dJointSetHingeParam(jointHinge, dParamVel, k1); // Set angular velocity[m/s]
//dJointSetHingeParam(jointHinge, dParamFMax, fMax); // Set max torque[N/m]
printf("\r %6d:",dJointGetHingeAngle(leg->jointHinge));
}
void draw::draw_tail(){
glPushMatrix();
//Link 1
glPushMatrix();
const dReal *tail_link_1_location = dBodyGetPosition(body_bag->getTailLink1Body());
glTranslatef(tail_link_1_location[0], tail_link_1_location[1], tail_link_1_location[2]);
const dReal *tail_link_1_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink1Body());
float tail_link_1_rotation_matrix_openGL[16];
getOpenGLRotationMatrix(tail_link_1_rotation_matrix_openGL, tail_link_1_rotation_matrix_ode);
//rotate the link
glMultMatrixf(tail_link_1_rotation_matrix_openGL);
glScalef(4, 4, 60);
draw_cube();
glPopMatrix();
//Link 2
glPushMatrix();
const dReal *tail_link_2_location = dBodyGetPosition(body_bag->getTailLink2Body());
glTranslatef(tail_link_2_location[0], tail_link_2_location[1], tail_link_2_location[2]);
const dReal *tail_link_2_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink2Body());
float tail_link_2_rotation_matrix_openGL[16];
getOpenGLRotationMatrix(tail_link_2_rotation_matrix_openGL, tail_link_2_rotation_matrix_ode);
//rotate the link
glMultMatrixf(tail_link_2_rotation_matrix_openGL);
glScalef(4, 4, 60);
draw_cube();
glPopMatrix();
//Link 3
glPushMatrix();
const dReal *tail_link_3_location = dBodyGetPosition(body_bag->getTailLink3Body());
glTranslatef(tail_link_3_location[0], tail_link_3_location[1], tail_link_3_location[2]);
const dReal *tail_link_3_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink3Body());
float tail_link_3_rotation_matrix_openGL[16];
getOpenGLRotationMatrix(tail_link_3_rotation_matrix_openGL, tail_link_3_rotation_matrix_ode);
//rotate the link
glMultMatrixf(tail_link_3_rotation_matrix_openGL);
glScalef(4, 4, 60);
draw_cube();
glPopMatrix();
//Link 4
glPushMatrix();
const dReal *tail_link_4_location = dBodyGetPosition(body_bag->getTailLink4Body());
glTranslatef(tail_link_4_location[0], tail_link_4_location[1], tail_link_4_location[2]);
const dReal *tail_link_4_rotation_matrix_ode = dBodyGetRotation(body_bag->getTailLink4Body());
float tail_link_4_rotation_matrix_openGL[16];
getOpenGLRotationMatrix(tail_link_4_rotation_matrix_openGL, tail_link_4_rotation_matrix_ode);
//rotate the link
glMultMatrixf(tail_link_4_rotation_matrix_openGL);
glScalef(4, 4, 60);
draw_cube();
glPopMatrix();
glPopMatrix();
}
void vmWishboneCar::drawLinkage(vm::WheelLoc loc)
{
// select wheel
vmWishbone *snow;
switch (loc) {
case vm::WheelLoc::FR:
snow= &frSuspension;
break;
case vm::WheelLoc::FL:
snow= &flSuspension;
break;
case vm::WheelLoc::RR:
snow= &rrSuspension;
break;
case vm::WheelLoc::RL:
snow= &rlSuspension;
break;
default:
break;
}
// setup linkage
const dReal *pos1,*R1,*pos2,*R2,*pos3,*R3;
const dReal *pos4,*R4,*pos5,*R5;
dsSetColorAlpha(0.0,0.0,1.0, 0.5);
pos1 = dBodyGetPosition(snow->uplink.body);
R1 = dBodyGetRotation(snow->uplink.body);
dsDrawCylinderD(pos1,R1,snow->uplink.width,snow->uplink.radius);
dsSetColor(0.0,1.0,0.0);
pos2 = dBodyGetPosition(snow->hlink.body);
R2 = dBodyGetRotation(snow->hlink.body);
dsDrawCylinderD(pos2,R2,snow->hlink.width,snow->hlink.radius);
dsSetColorAlpha(1.0,0.0,1.0, 0.5);
pos3 = dBodyGetPosition(snow->lowlink.body);
R3 = dBodyGetRotation(snow->lowlink.body);
dsDrawCylinderD(pos3,R3,snow->lowlink.width,snow->lowlink.radius);
// setup spring and damper
dsSetColorAlpha(0.1,0.7,1.0, 0.5);
pos4= dBodyGetPosition(snow->upstrut.body);
R4= dBodyGetRotation(snow->upstrut.body);
dsDrawCylinderD(pos4,R4,snow->upstrut.width,snow->upstrut.radius);
dsSetColorAlpha(1.0,0.7,1.0, 0.5);
pos5= dBodyGetPosition(snow->lowstrut.body);
R5= dBodyGetRotation(snow->lowstrut.body);
dsDrawCylinderD(pos5,R5,snow->lowstrut.width,snow->lowstrut.radius);
}
/*** ロボットアームの描画 ***/
void drawArm()
{
dReal r,length;
dReal box_length[3] = {0.4, 0.4, 0.4};
for (int i = 0; i < NUM-1; i++ ) { // カプセルの描画
dGeomCapsuleGetParams(rlink[i].geom, &r,&length);
dsDrawCapsule(dBodyGetPosition(rlink[i].body),
dBodyGetRotation(rlink[i].body),length,r);
}
dGeomBoxGetLengths(rlink[NUM-1].geom, box_length);
dsDrawBox(dBodyGetPosition(rlink[NUM-1].body), dBodyGetRotation(rlink[NUM-1].body), box_length);
}
/*** キーボードコマンドの処理関数 ***/
static void command(int cmd)
{
switch (cmd)
{
case '1':
float xyz1[3],hpr1[3];
dsGetViewpoint(xyz1,hpr1);
printf("xyz=%4.2f %4.2f %4.2f ",xyz1[0],xyz1[1],xyz1[2]);
printf("hpr=%6.2f %6.2f %5.2f \n",hpr1[0],hpr1[1],hpr1[2]);
break;
case 'k':
wheel[3].v -= 0.8;
break; // 後進
case 's':
wheel[0].v = wheel[1].v = wheel[2].v = wheel[3].v = 0.0; // 停止
break;
case 'x': // ゴロシュート
{
const dReal *bp = dBodyGetPosition(ball.body);
const dReal *p = dBodyGetPosition(wheel[0].body);
const dReal *R = dBodyGetRotation(base.body);
dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1], 2));
if (dist < 0.3)
{
dReal vmax = POWER * 0.01 * 8.0;
dBodySetLinearVel(ball.body,vmax * R[0],vmax * R[4], 0.0);
// printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]);
}
}
break;
case 'l': // ループシュート
{
const dReal *bp = dBodyGetPosition(ball.body);
const dReal *p = dBodyGetPosition(wheel[0].body);
const dReal *R = dBodyGetRotation(base.body);
dReal dist = sqrt(pow(bp[0]-p[0],2)+pow(bp[1]-p[1],2));
if (dist < 1.0)
{
dReal vmax45 = POWER * 0.01 * 8.0 / sqrt(2.0);
dBodySetLinearVel(ball.body,vmax45 * R[0],vmax45 * R[4], vmax45);
// printf("z:vx=%f vy=%f \n",vmax*R[0],vmax*R[4]);
}
}
break;
case 'b':
dBodySetPosition(ball.body,0,0,0.14+offset_z);
dBodySetLinearVel(ball.body,0,0,0);
dBodySetAngularVel(ball.body,0,0,0);
break;
}
}
vector<float> controller::getTargetPosition(int leg_id) {
//Last link
//translation
Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4);
mt(2, 3) = body_bag->getFootLinkLength(leg_id, 3);
//rotation
Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf point = Eigen::MatrixXf::Random(4, 1);
point(0, 0) = current_foot_location[leg_id][0];
point(1, 0) = current_foot_location[leg_id][1];
point(2, 0) = current_foot_location[leg_id][2];
point(3, 0) = 1;
Eigen::MatrixXf transformedPoint = mr*mt*mr.inverse()*point;
//Second last link
//translation
mt = Eigen::MatrixXf::Identity(4, 4);
mt(2, 3) = body_bag->getFootLinkLength(leg_id, 2);
//rotation
mr = Eigen::MatrixXf::Identity(4, 4);
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf endEffectorM = mr*mt*mr.inverse()*transformedPoint;
vector<float> endEffector(3);
endEffector[0] = endEffectorM(0,0);
endEffector[1] = endEffectorM(1,0);
endEffector[2] = endEffectorM(2,0);
return endEffector;
}
static void simLoop (int pause)
{
if (!pause) {
// add random forces and torques to all bodies
int i;
const dReal scale1 = 5;
const dReal scale2 = 5;
for (i=0; i<NUM; i++) {
dBodyAddForce (body[i],
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1));
dBodyAddTorque (body[i],
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1));
}
dWorldStep (world,0.05);
createTest();
}
// float sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}
/*** 台車の描画 ***/
static void drawBase()
{
// dsSetColor(1.3, 1.3, 0.0); // 黄色
// dsSetColor(0.0, 0.0, 1.3); // 青
dsSetColor(0.1, 0.1, 0.1); // 黒
dsDrawBox(dBodyGetPosition(base.body),dBodyGetRotation(base.body),SIDE);
}
void RigidBody::setMeshTransfomation()
{
if(m_type !=TRIANGLEMESH)
{
std::cerr<<"trying to set transform on non mesh\n";
return;
}
const dReal *rot=dBodyGetRotation(m_id);
const dReal *pos=dBodyGetPosition(m_id);
dReal tx[16];
tx[0]=rot[0];
tx[1]=rot[1];
tx[2]=rot[2];
tx[3]=pos[0];
tx[4]=rot[4];
tx[5]=rot[5];
tx[6]=rot[6];
tx[7]=pos[1];
tx[8]=rot[8];
tx[9]=rot[9];
tx[10]=rot[10];
tx[11]=pos[2];
tx[12]=rot[12];
tx[13]=rot[13];
tx[14]=rot[14];
tx[15]=1.0;
dGeomTriMeshSetLastTransform( m_geom.getID() ,tx );
dGeomTriMeshClearTCCache (m_geom.getID());
}
ngl::Mat4 RigidBody::getRotation()const
{
// When getting, the returned values are pointers to internal data structures,
// so the vectors are valid until any changes are made to the rigid body
// system structure.
const dReal *rot=dBodyGetRotation(m_id);
ngl::Mat4 m;
m.m_openGL[0]= rot[0];
m.m_openGL[1]= rot[4];
m.m_openGL[2]= rot[8];
m.m_openGL[3]= 0;
m.m_openGL[4]= rot[1];
m.m_openGL[5]= rot[5];
m.m_openGL[6]= rot[9];
m.m_openGL[7]= 0;
m.m_openGL[8]= rot[2];
m.m_openGL[9]= rot[6];
m.m_openGL[10]= rot[10];
m.m_openGL[11]= 0;
m.m_openGL[12]= rot[0];
m.m_openGL[13]= rot[1];
m.m_openGL[14]= rot[2];
m.m_openGL[15]= 1;
return m;
}
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);
}
}
/* returns the a-th principal axis of a body */
void body_axis (t_real *axis, dBodyID b, int a) {
if (b == SURFACE_BODY_ID) {
if (a==0) {
axis[0] = 0.;
axis[1] = 0.;
axis[2] = -1.;
}
else if (a==1) {
axis[0] = 0.;
axis[1] = 1.;
axis[2] = 0.;
}
else if (a==2) {
axis[0] = 1.;
axis[1] = 0.;
axis[2] = 0.;
}
}
else {
const t_real *o = dBodyGetRotation (b);
axis[0] = o[0+a];
axis[1] = o[4+a];
axis[2] = o[8+a];
}
}
/* return the principal inertia tensor of a body (inverse = 0) or its inverse (inverse = 1) */
void body_inertia_tensor (t_real *RIRt, dBodyID b, int inverse) {
const t_real *R;
t_matrix mi;
t_vector vi;
/* get principal inertia components */
body_inertia (vi, b);
if (inverse==0) {
mi[0] = vi[0]; mi[1] = 0. ; mi[2] = 0. ;
mi[4] = 0. ; mi[5] = vi[1]; mi[6] = 0. ;
mi[8] = 0. ; mi[9] = 0. ; mi[10] = vi[2];
}
else {
mi[0] = 1./vi[0]; mi[1] = 0. ; mi[2] = 0. ;
mi[4] = 0. ; mi[5] = 1./vi[1]; mi[6] = 0. ;
mi[8] = 0. ; mi[9] = 0. ; mi[10] = 1./vi[2];
}
/* fix extra elements */
mi[3] = mi[7] = mi[11] = 0.;
/* perform the matrix multiplication to bring it to the
* world reference frame */
R = dBodyGetRotation (b);
matrix_ABAt (RIRt, R, mi);
}
void DynamicsSolver::GetTransform ( dBodyID geom, Matrix* M )
{
float outmatrix[16];
dReal* L; dReal* R;
L = (dReal*)dBodyGetPosition(geom);
R = (dReal*)dBodyGetRotation(geom);
outmatrix[0]=(float)R[0];
outmatrix[1]=(float)R[4];
outmatrix[2]=(float)R[8];
outmatrix[3]=0;
outmatrix[4]=(float)R[1];
outmatrix[5]=(float)R[5];
outmatrix[6]=(float)R[9];
outmatrix[7]=0;
outmatrix[8]=(float)R[2];
outmatrix[9]=(float)R[6];
outmatrix[10]=(float)R[10];
outmatrix[11]=0;
outmatrix[12]=(float)L[0];
outmatrix[13]=(float)L[1];
outmatrix[14]=(float)L[2];
outmatrix[15]=1;
memcpy( M->Mat, outmatrix, sizeof(float) * 16);
}
void ODERigidObject::GetTransform(RigidTransform& T) const
{
const dReal* pos = dBodyGetPosition(bodyID);
Vector3 comPos(pos[0],pos[1],pos[2]);
const dReal* rot = dBodyGetRotation(bodyID);
CopyMatrix(T.R,rot);
T.t = comPos - T.R*obj.com;
}
static void simLoop (int pause)
{
int i;
if (!pause) {
// motor
dJointSetHinge2Param (joint[0],dParamVel2,-speed);
dJointSetHinge2Param (joint[0],dParamFMax2,0.1);
// steering
dReal v = steer - dJointGetHinge2Angle1 (joint[0]);
if (v > 0.1) v = 0.1;
if (v < -0.1) v = -0.1;
v *= 10.0;
dJointSetHinge2Param (joint[0],dParamVel,v);
dJointSetHinge2Param (joint[0],dParamFMax,0.2);
dJointSetHinge2Param (joint[0],dParamLoStop,-0.75);
dJointSetHinge2Param (joint[0],dParamHiStop,0.75);
dJointSetHinge2Param (joint[0],dParamFudgeFactor,0.1);
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
}
dsSetColor (0,1,1);
dsSetTexture (DS_WOOD);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides);
dsSetColor (1,1,1);
for (i=1; i<=3; i++) dsDrawCylinder (dBodyGetPosition(body[i]),
dBodyGetRotation(body[i]),0.02f,RADIUS);
dVector3 ss;
dGeomBoxGetLengths (ground_box,ss);
dsDrawBox (dGeomGetPosition(ground_box),dGeomGetRotation(ground_box),ss);
/*
printf ("%.10f %.10f %.10f %.10f\n",
dJointGetHingeAngle (joint[1]),
dJointGetHingeAngle (joint[2]),
dJointGetHingeAngleRate (joint[1]),
dJointGetHingeAngleRate (joint[2]));
*/
}
ODESpring *odeSpringCreate( dBodyID b0, dBodyID b1, FVec3 off0, FVec3 off1, int type, int computeNatLen ) {
for( int i=0; i<ODESPRINGS_MAX; i++ ) {
if( ! odeSprings[i].active ) {
odeSprings[i].type = type;
odeSprings[i].active = 1;
odeSprings[i].disabled = 0;
odeSprings[i].dBody[0] = b0;
odeSprings[i].dBody[1] = b1;
odeSprings[i].offset[0] = off0;
odeSprings[i].offset[1] = off1;
odeSprings[i].naturalLen = 0.f;
odeSprings[i].breakLen = 0.f;
odeSprings[i].maximumForce = 0.f;
odeSprings[i].constant = 1.f;
odeSprings[i].naturalLenPtr = 0;
odeSprings[i].breakLenPtr = 0;
odeSprings[i].maximumForcePtr = 0;
odeSprings[i].constantPtr = 0;
if( computeNatLen ) {
FVec3 p0 = odeSprings[i].offset[0];
if( odeSprings[i].dBody[0] ) {
dReal *pos0 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[0] );
dReal *rot0 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[0] );
FMat4 mat0 = zmathODERotPosToFMat4( rot0, pos0 );
p0 = mat0.mul( odeSprings[i].offset[0] );
}
FVec3 p1 = odeSprings[i].offset[1];
if( odeSprings[i].dBody[1] ) {
dReal *pos1 = (dReal *)dBodyGetPosition( odeSprings[i].dBody[1] );
dReal *rot1 = (dReal *)dBodyGetRotation( odeSprings[i].dBody[1] );
FMat4 mat1 = zmathODERotPosToFMat4( rot1, pos1 );
p1 = mat1.mul( odeSprings[i].offset[1] );
}
p0.sub( p1 );
odeSprings[i].naturalLen = p0.mag();
}
return &odeSprings[i];
}
}
assert( 0 );
return 0;
}
void TSRODERigidBody::DebugRender()
{
Graphics()->SetRasterizerState( Graphics()->m_FillWireFrameState );
TSRMatrix4 bodyTransform;
TSRMatrix4 geomTransform;
const float* pBodyPosition = dBodyGetPosition( m_BodyID );
const float* pBodyRotation = dBodyGetRotation( m_BodyID );
ODEToMatrix4( bodyTransform, pBodyPosition, pBodyRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
TSRDebugDraw::RenderAxis( 1.0f );
TSRDebugDraw::RenderSphere( 0.25f );
for ( unsigned int i = 0; i < m_GeomIDs.size(); i++ )
{
dGeomID currGeomTransformID = m_GeomIDs[ i ];
dGeomID geomID = dGeomTransformGetGeom( currGeomTransformID );
const float* pGeomPosition = dGeomGetPosition( geomID );
const float* pGeomRotation = dGeomGetRotation( geomID );
ODEToMatrix4( bodyTransform, pGeomPosition, pGeomRotation );
TSRGlobalConstants.PushMatrix();
TSRGlobalConstants.MultMatrix( bodyTransform.d );
switch( dGeomGetClass( geomID ) )
{
case dBoxClass:
{
dVector3 extents;
dGeomBoxGetLengths( geomID, extents );
TSRVector3 aabbMin( -extents[ 0 ], -extents[ 1 ], -extents[ 2 ] );
TSRVector3 aabbMax( +extents[ 0 ], +extents[ 1 ], +extents[ 2 ] );
aabbMin *= 0.5f;
aabbMax *= 0.5f;
TSRDebugDraw::RenderAABB( aabbMin, aabbMax );
}
break;
case dSphereClass:
{
float radius;
radius = dGeomSphereGetRadius( geomID );
TSRDebugDraw::RenderSphere( radius );
}
break;
case dCylinderClass:
{
float radius,length;
dGeomCylinderGetParams( geomID, &radius, &length );
TSRDebugDraw::RenderCylinder( length, radius, TSRVector3( 0.0f, 0.0f, 1.0f ) );
}
break;
}
TSRGlobalConstants.PopMatrix();
}
TSRGlobalConstants.PopMatrix();
Graphics()->SetRasterizerState( Graphics()->m_FillSolidState );
}
Pose Primitive::getPose() const {
if(!geom) {
if (!body)
return Pose::translate(0.0f,0.0f,0.0f); // fixes init bug
else
return osgPose(dBodyGetPosition(body), dBodyGetRotation(body));
}
return osgPose(dGeomGetPosition(geom), dGeomGetRotation(geom));
}
SpringHook::SpringHook(dBodyID _body,const Vector3& _worldpt,const Vector3& _target,Real _k)
:body(_body),target(_target),k(_k)
{
Matrix3 R;
Vector3 t;
CopyVector(t,dBodyGetPosition(body));
CopyMatrix(R,dBodyGetRotation(body));
R.mulTranspose(_worldpt-t,localpt);
}
void DrawBox()
{
for(int i = 0; i < BOX_NUM; i++)
{
dsSetColorAlpha(0.3, 0.2, 0.1, 1.0);
dsSetTexture(DS_WOOD);
dsDrawBoxD(dBodyGetPosition(box[i].body),dBodyGetRotation(box[i].body), boxSize[i]);
}
}
static void simLoop (int pause)
{
if (!pause) {
dSpaceCollide(space,0,&nearCallback);
dWorldQuickStep (world,0.05);
dJointGroupEmpty(contactgroup);
}
dReal sides1[3];
dGeomBoxGetLengths(geom[0], sides1);
dReal sides2[3];
dGeomBoxGetLengths(geom[1], sides2);
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);
}
// ロボットの描画
static void draw() {
const dReal *pos, *rot;
dReal side[3];
// SHIELDの描画
pos = dBodyGetPosition(rod[0].body);
rot = dBodyGetRotation(rod[0].body);
dsSetColor(1.0 ,0.0 ,0.0);
dsDrawCylinder(pos, rot, SHIELD_LENGTH, SHIELD_RADIUS);
// RODの描画
pos = dBodyGetPosition(rod[1].body);
rot = dBodyGetRotation(rod[1].body);
side[0] = ROD_WIDTH;
side[1] = ROD_WIDTH;
side[2] = ROD_LENGTH;
dsSetColor(0.9 ,0.7 ,0.13);
dsDrawBox(pos, rot, side);
// BODYの描画
pos = dBodyGetPosition(rod[2].body);
rot = dBodyGetRotation(rod[2].body);
side[0] = BODY_WIDTH;
side[1] = BODY_LENGTH;
side[2] = BODY_HEIGHT;
dsSetColor(0.0 ,0.0 ,1.0);
dsDrawBox(pos, rot, side);
// BULLETの描画
pos = dBodyGetPosition(bullet.body);
rot = dBodyGetRotation(bullet.body);
dsSetColor(0 ,0 ,0);
dsDrawSphere(pos, rot, BULLET_RADIUS+0.1);
// RAYの描画
dVector3 pos_s,pos_e;
pos_s[0] = CANNON_X; pos_s[1]=CANNON_Y; pos_s[2]=CANNON_Z;
pos_e[0] = CANNON_X + CANNON_Y * tan(cannon_q_d[0]);
pos_e[1] = 0.0;
pos_e[2] = CANNON_Z + CANNON_Y * tan(- 1.0 * cannon_q_d[1]) / cos(cannon_q_d[0]);
dsSetColor(1 ,0 ,0);
dsDrawLine(pos_s, pos_e);
}
