/*** 車輪の生成 ***/ void makeWheel() { dMass mass; dReal r = 0.1; dReal w = 0.024; dReal m = 0.15; dReal d = 0.01; dReal tmp_z = Z; dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0}; dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)}; dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z}; dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0}; dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2}; dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z}; for (int i = 0; i < WHEEL_NUM; i++) { wheel[i].v = 0.0; // make body, geom and set geom to body. // The position and orientation of geom is abondoned. wheel[i].body = dBodyCreate(world); wheel[i].geom = dCreateCylinder(space,r, w); dGeomSetBody(wheel[i].geom,wheel[i].body); // set configure of body dMassSetZero(&mass); if (i < 2) { dMassSetCylinderTotal(&mass,m, 1, r, w); } else { dMassSetCylinderTotal(&mass,m, 2, r, w); } dBodySetMass(wheel[i].body,&mass); // set position and orientation of body. dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]); dMatrix3 R; if (i >= 2) { dRFromAxisAndAngle(R,1,0,0,M_PI/2.0); dBodySetRotation(wheel[i].body,R); } else { dRFromAxisAndAngle(R,0,1,0,M_PI/2.0); dBodySetRotation(wheel[i].body,R); } // make joint. wheel[i].joint = dJointCreateHinge(world,0); dJointAttach(wheel[i].joint,base.body,wheel[i].body); if (i < 2) { dJointSetHingeAxis(wheel[i].joint, 1, 0, 0); } else { dJointSetHingeAxis(wheel[i].joint, 0, -1, 0); } dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]); } }
void PCylinder::setMass(dReal mass) { m_mass = mass; dMass m; dMassSetCylinderTotal (&m,m_mass,1,m_radius,m_length); dBodySetMass (body,&m); }
void cylinder::set_mass(double mass) { if(body_id) { dMass dmass; dMassSetZero(&dmass); dMassSetCylinderTotal(&dmass,mass,1,radius,length); dBodySetMass (body_id, &dmass); material.mass = mass; } }
void PhyCylinder::SetMass(float mass, int direction, float radius, float length) { if (mBodyId != NULL) { dMass m; dMassSetZero(&m); dMassSetCylinderTotal(&m, mass, direction, radius, length); dBodySetMass(mOdeBody, &m); } }
void cPhysicsObject::CreateCylinder(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot) { geom = dCreateCylinder(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(), fRadius, fLength); InitCommon(pWorld, pos, rot); if (bBody) { dMass mass; dMassSetCylinderTotal(&mass, fMassKg, uiDirectionX, 2.0f * fRadius, fLength); dBodySetMass(body, &mass); } }
void Wheel::createPhysics(Utils::Xml &x, dSpaceID space) { if (x.mustString("nature") == "sphere") { ph.geom = dCreateSphere(space, x.mustOReal("radius")); dMassSetSphereTotal(&ph.mass, x.mustOReal("mass"), x.mustOReal("radius")); } else { ph.geom = dCreateCylinder(space, x.mustOReal("radius"), x.mustOReal("width")); dMassSetCylinderTotal(&ph.mass, x.mustOReal("mass"), 1, x.mustOReal("radius"), x.mustOReal("width")); } ph.body = World::getSingletonPtr()->add(ph.geom, &ph.mass); }
BarrelBody(Barrel *b) : OdeBody(dBodyCreate(gWorld)), b_(b) { dMass m; dMassSetCylinderTotal(&m, b->mass_, 3, b->radius_, b->height_); dBodySetMass(id_, &m); Vec3 pos(b->pos()); dBodySetPosition(id_, pos.x + b->center_.x, pos.y + b->center_.y, pos.z + b->center_.z); dBodySetData(id_, this); dBodySetAutoDisableFlag(id_, 1); // This will roll forever, very slowly, unless I up the tolerance for sleep. // Compare to a box, which will come to a complete stop easily, and where we // don't want it stopping balancing on an edge. dBodySetAutoDisableAngularThreshold(id_, 0.03f); dBodySetAutoDisableLinearThreshold(id_, 0.03f); dBodySetAutoDisableAverageSamplesCount(id_, 5); }
void vmWishboneCar::buildWheelJointLinkage(vmWheel *wnow, vmWishbone *snow, dReal shiftSign) { // some parameters dMass m1; dMatrix3 rmat; const dReal *wheelPos= dBodyGetPosition(wnow->body); // uplink snow->uplink.body= dBodyCreate(world); dMassSetZero(&m1); dMassSetCylinderTotal(&m1,snow->uplink.mass,3 ,snow->uplink.radius ,snow->uplink.width); dBodySetMass(snow->uplink.body,&m1); dBodySetPosition(snow->uplink.body, wheelPos[0] ,wheelPos[1]+0.5*shiftSign*(snow->uplink.width) ,wheelPos[2]+0.5*snow->hlink.width); dRFromAxisAndAngle(rmat,1.0,0.0,0.0, 90*M_PI/180.0); dBodySetRotation(snow->uplink.body,rmat); // hlink snow->hlink.body= dBodyCreate(world); dMassSetZero(&m1); dMassSetCylinderTotal(&m1,snow->hlink.mass,3 ,snow->hlink.radius ,snow->hlink.width); dBodySetMass(snow->hlink.body,&m1); dBodySetPosition(snow->hlink.body, wheelPos[0] ,wheelPos[1],wheelPos[2]); // lowlink snow->lowlink.body= dBodyCreate(world); dMassSetZero(&m1); dMassSetCylinderTotal(&m1,snow->lowlink.mass,3 ,snow->lowlink.radius ,snow->lowlink.width); dBodySetMass(snow->lowlink.body,&m1); dBodySetPosition(snow->lowlink.body, wheelPos[0] ,wheelPos[1]+0.5*shiftSign*(snow->lowlink.width) ,wheelPos[2]-0.5*snow->hlink.width); dRFromAxisAndAngle(rmat,1.0,0.0,0.0, 90*M_PI/180.0); dBodySetRotation(snow->lowlink.body,rmat); // lowstrut dReal y,z,angle; angle= -shiftSign*strutAngle; y= wheelPos[1]+0.1*shiftSign*(snow->lowlink.width) +0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle)); z= wheelPos[2]-0.5*snow->hlink.width+ 0.5*snow->lowstrut.width*cos(angle); snow->lowstrut.body= dBodyCreate(world); dMassSetZero(&m1); dMassSetCylinderTotal(&m1,snow->lowstrut.mass,3 ,snow->lowstrut.radius,snow->uplink.width); dBodySetMass(snow->lowstrut.body, &m1); dBodySetPosition(snow->lowstrut.body,wheelPos[0],y,z); dRFromAxisAndAngle(rmat,1.0,0.0,0.0, angle); dBodySetRotation(snow->lowstrut.body,rmat); // upstrut snow->upstrut.body= dBodyCreate(world); dMassSetZero(&m1); dMassSetCylinderTotal(&m1,snow->upstrut.mass,3 ,snow->upstrut.radius,snow->uplink.width); dBodySetMass(snow->upstrut.body, &m1); y= wheelPos[1]+0.1*shiftSign*(snow->lowlink.width) +shiftSign*snow->lowstrut.width*fabs(sin(angle)) +0.5*shiftSign*snow->upstrut.width*fabs(sin(angle)); z= wheelPos[2]-0.5*snow->hlink.width +1.0*snow->lowstrut.width*cos(angle) +0.5*snow->upstrut.width*cos(angle); dBodySetPosition(snow->upstrut.body,wheelPos[0],y,z); dRFromAxisAndAngle(rmat,1.0,0.0,0.0, angle); dBodySetRotation(snow->upstrut.body,rmat); }
// ロボットの生成 void create() { // SHIELDの生成(空間に固定) rod[0].body = dBodyCreate(world); dBodySetPosition(rod[0].body, SHIELD_X, SHIELD_Y, SHIELD_Z); dMassSetZero(&mass); dMassSetCylinderTotal(&mass, SHIELD_WEIGHT, 2, SHIELD_RADIUS, SHIELD_LENGTH); dBodySetMass(rod[0].body, &mass); rod[0].geom = dCreateCylinder(space, SHIELD_RADIUS, SHIELD_LENGTH); dGeomSetBody(rod[0].geom, rod[0].body); dRFromAxisAndAngle(R, 1, 0, 0, 0.5 * M_PI); // x軸に90度回転 dBodySetRotation(rod[0].body, R); rod_joint[0] = dJointCreateFixed(world, 0); // 固定ジョイント dJointAttach(rod_joint[0], rod[0].body, 0); dJointSetFixed(rod_joint[0]); // RODの生成(回転ジョイントy軸に回転軸) rod[1].body = dBodyCreate(world); dBodySetPosition(rod[1].body, SHIELD_X, SHIELD_Y, SHIELD_Z); dMassSetZero(&mass); dMassSetBoxTotal(&mass, ROD_WEIGHT, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH); dBodySetMass(rod[1].body, &mass); rod[1].geom = dCreateBox(space, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH); dGeomSetBody(rod[1].geom, rod[1].body); dRFromAxisAndAngle(R, 0, 0, 1, 0.25 * M_PI); // z軸に45度回転 dBodySetRotation(rod[1].body, R); rod_joint[1] = dJointCreateHinge(world, 0); // ヒンジジョイント dJointAttach(rod_joint[1], rod[1].body, rod[0].body); dJointSetHingeAnchor(rod_joint[1], SHIELD_X, SHIELD_Y, SHIELD_Z); dJointSetHingeAxis(rod_joint[1], 0, 1, 0);// y軸ジョイント // BODYの生成(たてておくだけ) rod[2].body = dBodyCreate(world); dBodySetPosition(rod[2].body, BODY_X, BODY_Y, BODY_Z); dMassSetZero(&mass); dMassSetBoxTotal(&mass, BODY_WEIGHT, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT); dBodySetMass(rod[2].body, &mass); rod[2].geom = dCreateBox(space, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT); dGeomSetBody(rod[2].geom, rod[2].body); // BULLETの生成(CANNON中心に初期座標) bullet.body = dBodyCreate(world); dMassSetZero(&mass); dMassSetSphereTotal(&mass, BULLET_WEIGHT, BULLET_RADIUS); dBodySetMass(bullet.body,&mass); dBodySetPosition(bullet.body, CANNON_X, CANNON_Y, CANNON_Z); bullet.geom = dCreateSphere(space, BULLET_RADIUS); dGeomSetBody(bullet.geom, bullet.body); // TARGETの生成 // target.body = dBodyCreate(world); // dMassSetZero(&mass); // dMassSetSphereTotal(&mass, 0.0001, BULLET_RADIUS); // dBodySetMass(target.body,&mass); // dBodySetPosition(target.body, SHIELD_X, SHIELD_Y, SHIELD_Z); // target.geom = dCreateSphere(space, BULLET_RADIUS); // dGeomSetBody(target.geom, target.body); }
//! add ODE parts with fixed joint void SSimEntity::setMass(double mass) { m_parts.mass = mass; int geomNum = getGeomNum(); if (geomNum != 0) { // if a part is already added // mass per part double ms = mass / geomNum; // refer geometry and body dBodyID body = m_parts.body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // refer the type of geometory dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } const dReal *pos = dGeomGetPosition(geom); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } /* for (int i = 0; i < geomNum; i++) { dGeomID geom = dGeomTransformGetGeom(m_parts.geoms[i]); const dReal *pos = dGeomGetPosition(geom); // Change to a relative position from CoG dGeomSetPosition(geom, pos[0] - m.c[0], pos[1] - m.c[1], pos[2] - m.c[2]); } // keep the CoG position //m_parts.pos.set(m.c[0], m.c[1], m.c[2]); */ // Adjustment of the gap from CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // Setting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
//! setting of mass void SSimRobotEntity::setMass(SSimObjParts *parts, double mass) { // if a part is already added int geomNum = parts->geoms.size(); if (geomNum != 0) { // mass per each part double ms = mass / geomNum; // refer geometry and body dBodyID body = parts->body; dMass m; dMass m2; dMassSetZero(&m); dMassSetZero(&m2); for (int i = 0; i < geomNum; i++) { // Refer the type of geometry dGeomID geom = dGeomTransformGetGeom(parts->geoms[i]); int type = dGeomGetClass(geom); // setting of mass // sphere if (type == 0) { dReal radius = dGeomSphereGetRadius(geom); dMassSetSphereTotal(&m2, ms, radius); } // box else if (type == 1) { dVector3 size; dGeomBoxGetLengths(geom, size); dMassSetBoxTotal(&m2, ms, size[0], size[1], size[2]); } // cylinder else if (type == 3) { dReal radius = 0.0; dReal length = 0.0; dGeomCylinderGetParams(geom, &radius, &length); // TODO: confirm: Is 2 suitable for long axis? dMassSetCylinderTotal(&m2, ms, 2, radius, length); } // const dReal *pos = dGeomGetPosition(geom); //LOG_MSG(("pos = (%f, %f, %f)", pos[0], pos[1], pos[2])); dMassTranslate(&m2, pos[0], pos[1], pos[2]); dMassAdd(&m, &m2); } // adjustment of the gap between CoG //const dReal *p = dBodyGetPosition(body); //dBodySetPosition(body,p[0]+m.c[0], p[1]+m.c[1], p[2]+m.c[2]); dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); // seeting of mass dBodySetMass(body, &m); dBodySetDamping(body, 0.8, 0.8); // added by inamura on 2014-01-29 for test } // if (partsNum != 0) { }
void dMassSetCylinder (dMass *m, dReal density, int direction, dReal radius, dReal length) { dMassSetCylinderTotal (m, (dReal) (M_PI*radius*radius*length*density), direction, radius, length); }
/* -------------------------------- ** 全方向移動ロボットの生成, 描画 ----------------------------------- */ void MakeOmni() { /* ローカル変数の定義 */ dMass mass; double r, w, d, m_w, x, y, z, lx, ly, lz, m_b; /* ロボットの個数分だけループ */ for (int i=0; i< OMNI_NUM; i++) { /* 車輪 */ r = wheelSize[i][0]; w = wheelSize[i][1]; d = wheelSize[i][2]; m_w = wheelM[i]; /* 土台 */ x = basePos[i][0]; y = basePos[i][1]; z = basePos[i][2]; lx = baseSize[i][0]; ly = baseSize[i][1]; lz = baseSize[i][2]; m_b = baseM[i]; /* 土台の生成 */ base[i].body = dBodyCreate(world); dMassSetZero(&mass); dMassSetBoxTotal(&mass, m_b, lx, ly, lz); dBodySetMass(base[i].body,&mass); base[i].geom = dCreateBox(space, lx, ly, lz); dGeomSetBody(base[i].geom, base[i].body); dBodySetPosition(base[i].body, x, y, z); /* 座標 */ /* 車輪の生成 */ double wx[WHEEL_NUM] = {lx/2+w/2+d, - (lx/2+w/2+d), 0, 0}; double wy[WHEEL_NUM] = {0, 0, ly/2+w/2+d, -(ly/2+w/2+d)}; double wz[WHEEL_NUM] = {z, z, z, z}; double jx[WHEEL_NUM] = {lx/2, -lx/2, 0, 0}; double jy[WHEEL_NUM] = {0, 0, ly/2, -ly/2}; double jz[WHEEL_NUM] = {z, z, z, z}; for (int j = 0; j < WHEEL_NUM; j++) { wheel[i][j].body = dBodyCreate(world); dMatrix3 R; if (j >= 2) { dRFromAxisAndAngle(R,1,0,0,M_PI/2.0); dBodySetRotation(wheel[i][j].body,R); } else { dRFromAxisAndAngle(R, 0, 1, 0, M_PI/2.0); dBodySetRotation(wheel[i][j].body, R); } dMassSetZero(&mass); if (j < 2) { dMassSetCylinderTotal(&mass, m_w, 1, r, w); } else { dMassSetCylinderTotal(&mass, m_w, 2, r, w); } dBodySetMass(wheel[i][j].body,&mass); wheel[i][j].geom = dCreateCylinder(space, r, w); dGeomSetBody(wheel[i][j].geom, wheel[i][j].body); dBodySetPosition(wheel[i][j].body, wx[j], wy[j], wz[j]); wheel[i][j].joint = dJointCreateHinge(world, 0); dJointAttach(wheel[i][j].joint, base[i].body, wheel[0][j].body); if (j < 2) { dJointSetHingeAxis(wheel[i][j].joint, 1, 0, 0); } else { dJointSetHingeAxis(wheel[i][j].joint, 0, -1, 0); } dJointSetHingeAnchor(wheel[i][j].joint, jx[j], jy[j], jz[j]); } } }