void CRigidBox::setMass(F32 mass){
if(!mBodyID)
return;
dMass m;
dMassSetBoxTotal(&m, mass, TO_PHYSICS(mDimentions.x), TO_PHYSICS(mDimentions.y), TO_PHYSICS(mDimentions.z));
dBodySetMass(mBodyID, &m);
}
void trimesh::set_mass(double mass)
{
if(body_id)
{
dMass dmass;
dMassSetBoxTotal (&dmass, mass, size[0], size[1], size[2]);
dBodySetMass (body_id, &dmass);
material.mass = mass;
}
}
IoObject *IoODEMass_setBoxMass(IoODEMass *self, IoObject *locals, IoMessage *m)
{
const double totalMass = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double lx = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double ly = IoMessage_locals_doubleArgAt_(m, locals, 2);
const double lz = IoMessage_locals_doubleArgAt_(m, locals, 3);
dMassSetBoxTotal(DATA(self), totalMass, lx, ly, lz);
return self;
}
void OscPrismODE::on_mass()
{
ODEObject *ode_object = static_cast<ODEObject*>(special());
dMassSetBoxTotal(&ode_object->mass(), m_mass.m_value,
m_size.x, m_size.y, m_size.z);
dBodySetMass(ode_object->body(), &ode_object->mass());
dReal volume = m_size.x * m_size.y * m_size.z;
m_density.m_value = m_mass.m_value / volume;
}
void cPhysicsObject::CreateBox(cWorld* pWorld, const math::cVec3& pos, const math::cVec3& rot)
{
geom = dCreateBox(bDynamic ? pWorld->GetSpaceDynamic() : pWorld->GetSpaceStatic(),
2.0f * fWidth, 2.0f * fLength, 2.0f * fHeight);
InitCommon(pWorld, pos, rot);
if (bBody) {
dMass mass;
dMassSetBoxTotal(&mass, fMassKg, 2.0f * fWidth, 2.0f * fLength, 2.0f * fHeight);
dBodySetMass(body, &mass);
}
}
TouchSensor::TouchSensor(dSpaceID odeSpace, dBodyID sensorBody, float mass,
osg::Vec3 pos, float x, float y, float z, std::string label) :
collideSpace_(odeSpace), label_(label) {
// create own space to avoid physical collisions with other objects
sensorSpace_ = dHashSpaceCreate(0);
// create Sensor geom in this different space
dMass massOde;
dMassSetBoxTotal(&massOde, mass, x, y, z);
dBodySetMass(sensorBody, &massOde);
sensorGeometry_ = dCreateBox(sensorSpace_, x, y, z);
dBodySetPosition(sensorBody, pos.x(), pos.y(), pos.z());
dGeomSetPosition(sensorGeometry_, pos.x(), pos.y(), pos.z());
dGeomSetBody(sensorGeometry_, sensorBody);
}
//set mass and sides by available dimensions for box objects
void ODE_Particle::setMassTotal(dReal total_mass, dReal side1,dReal side2,dReal side3)
{
dMass m;
dMassSetZero(&m);
if (getShapeType() == dBoxClass)
{
dMassSetBoxTotal(&m,total_mass,side1,side2,side3);
dBodySetMass(body,&m);
}
else
{
printf("ERROR in ODE_Particle.cpp: Setting Mass using total mass for non box object");
exit(0);
}
}
/*** 台車の生成 ***/
static void makeBase()
{
dMass mass;
base.x = base.y = 0.0;
base.z = Z;
base.lx = SIDE[0]; base.ly = SIDE[1]; base.lz = SIDE[2];
base.body = dBodyCreate(world);
base.geom = dCreateBox(space,base.lx, base.ly, base.lz);
dGeomSetBody(base.geom, base.body);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,M,base.lx, base.ly, base.lz);
dBodySetMass(base.body,&mass);
dBodySetPosition(base.body, base.x, base.y, base.z);
}
/*** ロボットアームの生成 ***/
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]);
}
}
CDynamics3DBox::CDynamics3DBox(CDynamics3DEngine& c_engine,
CBoxEntity& c_box) :
CDynamics3DEntity(c_engine, c_box.GetEmbodiedEntity()),
m_cBoxEntity(c_box),
m_sGeomData(GEOM_NORMAL) {
/* Check whether the box is movable or not */
if(c_box.GetEmbodiedEntity().IsMovable()) {
/* Movable box */
/* Set the body to its initial position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dBodySetQuaternion(m_tBody, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dBodySetPosition(m_tBody, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Create the geometry and the mass */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Set Geom gripping properties. */
m_sGeomData.Type = GEOM_GRIPPABLE;
dGeomSetData(m_tGeom, &m_sGeomData);
/* Create its mass */
dMassSetBoxTotal(&m_tMass, c_box.GetMass(), cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
/* Associate the body to the geom */
dGeomSetBody(m_tGeom, m_tBody);
/* Set the parent body total mass */
dBodySetMass(m_tBody, &m_tMass);
}
else {
/* Unmovable box, get rid of the body and add only the geometry */
dBodyDestroy(m_tBody);
/* Create the geometry */
const CVector3& cBoxSize = c_box.GetSize();
m_tGeom = dCreateBox(m_tEntitySpace, cBoxSize.GetX(), cBoxSize.GetY(), cBoxSize.GetZ());
dGeomSetData(m_tGeom, &m_sGeomData);
/* Set the geom to its position and orientation */
const CQuaternion& cOrient = GetEmbodiedEntity().GetOrientation();
dQuaternion tQuat = { cOrient.GetW(), cOrient.GetX(), cOrient.GetY(), cOrient.GetZ() };
dGeomSetQuaternion(m_tGeom, tQuat);
const CVector3& cPos = GetEmbodiedEntity().GetPosition();
dGeomSetPosition(m_tGeom, cPos.GetX(), cPos.GetY(), cPos.GetZ());
/* Associate the geom to null body (this makes it static) */
dGeomSetBody(m_tGeom, 0);
}
}
void dmCreateBox(dmObject *obj, double p[3], double R[12], double m, double side[3], double color[3])
{
obj->body = dBodyCreate(world); // ボールの生成
obj->m = m;
obj->R = R;
obj->p = p;
obj->side = side;
obj->color = color;
dMass mass; // 構造体massの宣言
dMassSetZero(&mass); // 構造体massの初期化
dMassSetBoxTotal(&mass,obj->m,obj->side[0], obj->side[1], obj->side[2]); // 構造体massに質量を設定
dBodySetMass(obj->body,&mass); // ボールにmassを設定
dBodySetPosition(obj->body, obj->p[0], obj->p[1], obj->p[2]); // ボールの位置(x,y,z)を設定
dBodySetRotation(obj->body, obj->R);
obj->geom = dCreateBox(space,obj->side[0], obj->side[1], obj->side[2]); // 球ジオメトリの生成
dGeomSetBody(obj->geom, obj->body); // ボディとジオメトリの関連付け
}
void HarrierSim::makeHarrier()
{
dMass mass;
itsHarrierBody = dBodyCreate(world);
//pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;
dBodySetPosition(itsHarrierBody,0,0.2*5,4.94);
dMatrix3 R;
dRFromAxisAndAngle (R,1,0,0,0);
dBodySetRotation(itsHarrierBody, R);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, itsHarrierWeight,itsHarrierWidth, itsHarrierLength, 0.5);
//dMassSetCappedCylinderTotal(&mass,itsHarrierWeight,3,itsHarrierWidth,itsHarrierLength/2);
dMassRotate(&mass, R);
dBodySetMass(itsHarrierBody,&mass);
itsHarrierGeom = dCreateBox(space, itsHarrierWidth, itsHarrierLength, 0.5);
dGeomSetRotation(itsHarrierGeom, R);
dGeomSetBody(itsHarrierGeom, itsHarrierBody);
}
SceneObj::SceneObj( Scene *scene_p )
{
m_scene_p = scene_p;
m_prev_p = 0;
m_next_p = m_scene_p->m_obj_p;
if( m_next_p )
m_next_p->m_prev_p = this;
m_scene_p->m_obj_p = this;
m_dbody = dBodyCreate( m_scene_p->m_dworld );
dBodySetData( m_dbody, this );
dBodyDisable( m_dbody );
dMass mass;
dMassSetBoxTotal( &mass, 0.1f, 57, 57, 20 );
dBodySetMass( m_dbody, &mass );
m_dgeom = dCreateBox( m_scene_p->m_dspace, 57, 57, 20 );
dGeomSetBody( m_dgeom, m_dbody );
m_texid = 0;
}
/* ------------------------
* 箱の生成,描画
------------------------ */
void MakeBox()
{
/* ローカル変数の定義 */
dMass mass;
for(int i = 0; i < BOX_NUM; i++)
{
/* ボディを生成 */
box[i].body = dBodyCreate(world);
dMassSetZero(&mass);
/* 質量を設定 */
dMassSetBoxTotal(&mass, boxM[i], boxSize[i][0], boxSize[i][1], boxSize[i][2]);
dBodySetMass(box[i].body, &mass);
/* ジオメトリ生成 */
box[i].geom = dCreateBox(space, boxSize[i][0], boxSize[i][1], boxSize[i][2]);
/* ジオメトリをセット */
dGeomSetBody(box[i].geom, box[i].body);
dBodySetPosition(box[i].body, boxPos[i][0], boxPos[i][1], boxPos[i][2]);
/* 箱と地面の結合 */
fixed[i] = dJointCreateFixed(world, 0);
dJointAttach(fixed[i], box[i].body, 0);
dJointSetFixed(fixed[i]);
}
}
void add_phys_mass(dMass *mass, dGeomID geom, const float p[3],
const float r[16])
{
dVector3 v;
dMatrix3 M;
dReal rad;
dReal len;
dMass add;
if (r) set_rotation(M, r);
if (dGeomGetClass(geom) != dPlaneClass)
{
dReal m = get_data(geom)->mass;
/* Create a new mass for the given geom. */
switch (dGeomGetClass(geom))
{
case dBoxClass:
dGeomBoxGetLengths(geom, v);
dMassSetBoxTotal(&add, m, v[0], v[1], v[2]);
break;
case dSphereClass:
rad = dGeomSphereGetRadius(geom);
dMassSetSphereTotal(&add, m, rad);
break;
case dCapsuleClass:
dGeomCapsuleGetParams(geom, &rad, &len);
dMassSetCapsuleTotal(&add, m, 3, rad, len);
break;
default:
dMassSetZero(&add);
break;
}
/* Transform the geom and mass to the given position and rotation. */
if(dGeomGetBody(geom))
{
if (p)
{
dGeomSetOffsetPosition(geom, p[0], p[1], p[2]);
dMassTranslate (&add, p[0], p[1], p[2]);
}
if (r)
{
dGeomSetOffsetRotation(geom, M);
dMassRotate (&add, M);
}
}
else
{
if (p) dGeomSetPosition(geom, p[0], p[1], p[2]);
if (r) dGeomSetRotation(geom, M);
}
/* Accumulate the new mass with the body's existing mass. */
dMassAdd(mass, &add);
}
}
/* --------------------------------
** 全方向移動ロボットの生成, 描画
----------------------------------- */
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]);
}
}
}
void dMassSetBox (dMass *m, dReal density,
dReal lx, dReal ly, dReal lz)
{
dMassSetBoxTotal (m, lx*ly*lz*density, lx, ly, lz);
}
void makeRobot_Nleg()
{
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
dReal torso_m = 50.0; // Mass of body
// torso_m[segment] = 10.0;
dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments
//for four legs
// dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
// {-cx1,-cx1,-cx1}};
dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
{0,0,0}};
dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
{-cy1,-cy1,-cy1}};
dReal z[num_legs][num_links] = { // Position of each link (z coordinate)
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
dReal r[num_links] = { r1, r2, r3}; // radius of leg segment
dReal length[num_links] = { l1, l2, l3}; // Length of leg segment
dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment
// //for one leg
// dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
// dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
// dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
//for four legs
dReal axis_x[num_legs][num_links];
dReal axis_y[num_legs][num_links];
dReal axis_z[num_legs][num_links];
for(int i = 0; i < num_legs; ++i) {
axis_x[i][0] = 0.0;
axis_x[i][1] = 1.0;
axis_x[i][2] = 0.0;
axis_y[i][0] = 1.0;
axis_y[i][1] = 0.0;
axis_y[i][2] = 1.0;
axis_z[i][0] = 0.0;
axis_z[i][1] = 0.0;
axis_z[i][2] = 0.0;
}
// For mation of the body
dMass mass;
torso[segment].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
dBodySetMass(torso[segment].body,&mass);
torso[segment].geom = dCreateBox(space,lx, ly, lz);
dGeomSetBody(torso[segment].geom, torso[segment].body);
dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ);
// Formation of leg
dMatrix3 R; // Revolution queue
dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
THETA[segment][i][j] = 0;
leg[segment][i][j].body = dBodyCreate(world);
if (j == 0)
dBodySetRotation(leg[segment][i][j].body,R);
dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
dBodySetMass(leg[segment][i][j].body, &mass);
//if(i==1 and j==2) //to set the length of one leg differently
//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
//else
leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
}
}
// Formation of joints (and connecting them up)
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[segment][i][j].joint = dJointCreateHinge(world, 0);
if (j == 0){
dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop, .50*M_PI);
}
else
dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
}
}
}
#ifdef USE_NLEG_ROBOT
// link torsos
for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
switch (hingeType) {
case 1: //Hinge Joint, X axis (back-breaker)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
break;
case 2: //Hinge Joint, Y axis (???)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
break;
case 3: //Hinge Joint, Z axis (snake-like)
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
break;
case 4: // Slider, Y axis (??)
torso[segment].joint = dJointCreateSlider(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
break;
case 5: // Slider, X axis (extendo)
torso[segment].joint = dJointCreateSlider(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);
break;
case 6: //Universal Joint
torso[segment].joint = dJointCreateUniversal(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);
break;
case 7: //Ball Joint
torso[segment].joint = dJointCreateBall(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
break;
case 8: // Fixed
torso[segment].joint = dJointCreateHinge(world, 0);
dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(torso[segment].joint, dParamHiStop, 0.00*M_PI);
break;
default:
assert(false); // not a valid hinge type
break;
}
}
#endif
}
State* init() {
State* state = new State();
dInitODE();
SDL_Init(SDL_INIT_EVERYTHING);
state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL);
SDL_WM_SetCaption("Physics", NULL);
SDL_Flip(state->screen);
SDL_ShowCursor(SDL_DISABLE);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_LIGHTING);
glEnable(GL_LIGHT0);
GLfloat light_ambient[] = { 1, 1, 1, 1 };
glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient);
GLfloat lightpos[] = {0, 4, 0, 1};
glLightfv(GL_LIGHT0, GL_POSITION, lightpos);
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
glShadeModel(GL_SMOOTH);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_DEPTH_TEST);
glEnable(GL_CULL_FACE);
glClearColor(0, 0, 0, 1);
state->posx = 0;//21;
state->posy = 4;//8;
state->posz = 5;//21;
state->rotx = 0;
state->roty = 0;//-40;
state->rotz = 0;
state->wkey = false;
state->akey = false;
state->skey = false;
state->dkey = false;
state->gkey = false;
state->simSpeed = 60;
state->carcam = false;
state->carbodydrawable = new Drawable("objs/carbody.obj");
state->carwheeldrawable = new Drawable("objs/carwheel.obj");
state->map = new Drawable("objs/jump2.obj");
state->cube = new Drawable("objs/cube.obj");
// state->monkey = new Drawable("objs/monkey.obj");
state->world = dWorldCreate();
dWorldSetGravity(state->world, 0, -9.8, 0);
state->worldSpace = dHashSpaceCreate(0);
const double carHeight = 0.65;
const double carZ = 90;
const double carX = 0;
const float speed = -1000;
const float force = 200;
state->carbodybody = dBodyCreate(state->world);
dBodySetPosition(state->carbodybody, carX, carHeight, carZ);
dMass bodymass;
dMassSetBoxTotal(&bodymass, 100, 2, 4, 1);
dBodySetMass(state->carbodybody, &bodymass);
dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4);
dGeomSetBody(carbodygeom, state->carbodybody);
state->flcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2);
const dMatrix3 m = { 0, 0, 1, 0
, 0, 1, 0, 0
, 0, 0, 1, 0 };
dBodySetRotation(state->flcarwheelbody, m);
dMass wheelmass;
dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2);
dBodySetMass(state->flcarwheelbody, &wheelmass);
dJointID joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->flcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(flcarwheelgeom, state->flcarwheelbody);
dJointID motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->flcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->frcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2);
dBodySetRotation(state->frcarwheelbody, m);
dBodySetMass(state->frcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->frcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(frcarwheelgeom, state->frcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->frcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->blcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->blcarwheelbody, m);
dBodySetMass(state->blcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->blcarwheelbody);
dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(blcarwheelgeom, state->blcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->blcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->brcarwheelbody = dBodyCreate(state->world);
dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2);
dBodySetRotation(state->brcarwheelbody, m);
dBodySetMass(state->brcarwheelbody, &wheelmass);
joint = dJointCreateHinge(state->world, 0);
dJointAttach(joint, state->carbodybody, state->brcarwheelbody);
dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2);
dJointSetHingeAxis(joint, 1, 0, 0);
dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2);
dGeomSetBody(brcarwheelgeom, state->brcarwheelbody);
motor = dJointCreateAMotor(state->world, 0);
dJointAttach(motor, state->brcarwheelbody, state->carbodybody);
dJointSetAMotorNumAxes(motor, 1);
dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0);
dJointSetAMotorParam(motor, dParamVel, speed);
dJointSetAMotorParam(motor, dParamFMax, force);
state->var = new double[3];
state->var = dBodyGetPosition(state->carbodybody);
// cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl;
//TODO check if translation matrix from dBody can be used.
dSpaceID cubespace = dHashSpaceCreate(state->worldSpace);
for(int i = 0; i < NUM_CUBES/10; i++) {
for(int k = 0; k < 10; k++) {
state->cubebody[i*10+k] = dBodyCreate(state->world);
dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1);
dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70);
dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2);
dGeomSetBody(cubegeom, state->cubebody[i*10+k]);
}
}
{
int indexSize = state->map->vertices.size()/3;
unsigned int* index = new unsigned int[indexSize];
for(int i = 0; i < indexSize; i++)
index[i] = i;
dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12);
dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
dGeomSetPosition(mapGeom, 0, 0, 0);
}
// state->monkeyBody = dBodyCreate(state->world);
// {
// int indexSize = state->monkey->vertices.size()/3;
// unsigned int* index = new unsigned int[indexSize];
// for(int i = 0; i < indexSize; i++)
// index[i] = i;
// dTriMeshDataID triMeshData = dGeomTriMeshDataCreate();
// dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12);
// dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL);
// dGeomSetPosition(monkeyGeom, 0, 2, 0);
// dGeomSetBody(monkeyGeom, state->monkeyBody);
// }
state->physicsContactgroup = dJointGroupCreate(0);
return state;
}
void odCable::setupBody(dWorldID *world, dSpaceID space){
int i;
double pi=4.*atan(1.);
double segLen, dL, x;
double dd, s, rs;
dVector3 OP;
odPoint loc1, loc2, d, P, c;
dMass *m;
dGeomID geometry[nSegments];
// dQuaternion align;
dReal align[4];
printf("Setting up cable model\n");
if (body1!=NULL){
loc1=body1->globalPosition(pos1);
}else{
loc1=pos1;
}
if (body2!=NULL){
loc2=body2->globalPosition(pos2);
}else{
loc2=pos2;
}
d=loc2-loc1;
dL=d.mag()/(double)nSegments;
segLen=length/(double)nSegments;
printf("Distance = %lf m\n",d.mag());
printf("Segment length = %lf m\n",segLen);
printf("Allocating memory\n");
element = new dBodyID[nSegments];
joint = new dJointID[nSegments-1];
printf("Creating elements\n");
for (i=0;i<nSegments;i++){
element[i] = dBodyCreate (*world);
dBodySetDamping(element[i], linearDamping, angularDamping);
m=new dMass;
dMassSetBoxTotal(m, weight*segLen, segLen, diameter, diameter);
dMassAdjust (m, weight*segLen);
dBodySetMass (element[i], m);
x=((double)i+.5)*segLen;
dBodySetPosition(element[i], x, 0, 0);
// geometry[i] = dCreateBox(space, length, diameter, diameter);
// dGeomSetBody(geometry[i], element[i]);
}
printf("Creating internal joints\n");
for (i=0;i<nSegments-1;i++){
joint[i] = dJointCreateBall(*world,0);
dJointAttach (joint[i],element[i],element[i+1]);
x = ((double)i+1.)*segLen;
dJointSetBallAnchor (joint[i],x,0,0);
}
/*
c = d.crossProduct(odPoint(0,0,1));
dd = d.dotProduct_natural(odPoint(0,0,1));
s = sqrt((1.0 + dd)*2.0);
rs = 1.0/s;
align[0]=s*0.5; align[1]=c.x*rs; align[2]=c.y*rs; align[3]=c.z*rs;
printf("Moving segments to world positions...\n");
for (i=0;i<nSegments;i++){
P=loc1+d*((double)i)/(nSegments-1);
if (i==0) P.x+=segLen/2.;
if (i==nSegments-1) P.x-=segLen/2.;
dBodySetPosition(element[i], P.x, P.y, P.z);
// dBodySetQuaternion(element[i], align);
}
*/
printf("Creating end joint 1\n");
end1 = dJointCreateBall(*world,0);
if (body1!=NULL){
dJointAttach(end1, body1->odeBody, element[0]);
}else{
dJointAttach(end1, 0, element[0]);
}
// dJointSetBallAnchor (end1, loc1.x, loc1.y, loc1.z);
dJointSetBallAnchor (end1, 0, 0, 0);
end1Feedback = new dJointFeedback;
dJointSetFeedback (end1, end1Feedback);
/*
printf("Creating end joint 2\n");
end2 = dJointCreateBall(*world,0);
if (body2!=NULL){
dJointAttach(end2, body2->odeBody, element[nSegments-1]);
}else{
dJointAttach(end2, 0, element[nSegments-1]);
}
// dJointSetBallAnchor (end2, loc2.x, loc2.y, loc2.z);
dJointSetBallAnchor (end2, nSegments*segLen, 0, 0);
*/
end2Feedback = new dJointFeedback;
dJointSetFeedback (joint[0], end2Feedback);
}
// ロボットの生成
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) {
}
void PhysicsBody::setBoxMassTotal( Real32 totalMass, Real32 lx, Real32 ly, Real32 lz )
{
dMass mass;
dMassSetBoxTotal(&mass, totalMass, lx, ly, lz);
setMassStruct(mass);
}
void BoxMass::assembleMass()
{
dMassSetBoxTotal(&mass, value, depth, width, height);
}
/*** Formation of robot ***/
void makeRobot()
{
dReal torso_m = 10.0; // Mass of body
dReal l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments
//for four legs
dReal x[num_legs][num_links] = {{ cx1, cx1, cx1},{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
{-cx1,-cx1,-cx1},{ cx1, cx1, cx1}};
dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},{ cy1, cy1, cy1},// Position of each link (y coordinate)
{-cy1,-cy1,-cy1},{-cy1,-cy1,-cy1}};
dReal z[num_legs][num_links] = { // Position of each link (z coordinate)
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}};
dReal r[num_links] = { r1, r2, r3}; // radius of leg segment
dReal length[num_links] = { l1, l2, l3}; // Length of leg segment
dReal weight[num_links] = {l1m,l2m,l3m}; // Mass of leg segment
// //for one leg
// dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
// dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
// dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
//for four legs
dReal axis_x[num_legs][num_links] = {{ 0,1, 0},{ 0,1,0},{ 0, 1, 0},{ 0, 1, 0}};
dReal axis_y[num_legs][num_links] = {{ 1,0, 1},{ 1,0,1},{ 1, 0, 1},{ 1, 0, 1}};
dReal axis_z[num_legs][num_links] = {{ 0,0, 0},{ 0,0,0},{ 0, 0, 0},{ 0, 0, 0}};
// For mation of the body
dMass mass;
torso[0].body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
dBodySetMass(torso[0].body,&mass);
torso[0].geom = dCreateBox(space, lx, ly, lz);
dGeomSetBody(torso[0].geom, torso[0].body);
dBodySetPosition(torso[0].body, SX, SY, SZ);
// Formation of leg
dMatrix3 R; // Revolution queue
dRFromAxisAndAngle(R,1,0,0,M_PI/2); // 90 degrees to turn, parallel with the land
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[0][i][j].body = dBodyCreate(world);
if (j == 0) dBodySetRotation(leg[0][i][j].body,R);
dBodySetPosition(leg[0][i][j].body, SX+x[i][j], SY+y[i][j], SZ+z[i][j]);
dMassSetZero(&mass);
dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
dBodySetMass(leg[0][i][j].body, &mass);
//if(i==1 and j==2) //to set the length of one leg differently
//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
//else
leg[0][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
dGeomSetBody(leg[0][i][j].geom,leg[0][i][j].body);
}
}
// Formation of joints (and connecting them up)
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
leg[0][i][j].joint = dJointCreateHinge(world, 0);
if (j == 0){
dJointAttach(leg[0][i][j].joint, torso[0].body, leg[0][i][j].body); //connects hip to the environment
dJointSetHingeParam(leg[0][i][j].joint, dParamLoStop, -.5*M_PI); //prevent the hip forward-back from going more than 90 degrees
dJointSetHingeParam(leg[0][i][j].joint, dParamHiStop, .5*M_PI);
}
else
dJointAttach(leg[0][i][j].joint, leg[0][i][j-1].body, leg[0][i][j].body);
dJointSetHingeAnchor(leg[0][i][j].joint, SX+c_x[i][j], SY+c_y[i][j],SZ+c_z[i][j]);
dJointSetHingeAxis(leg[0][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
}
}
}
//! 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 BoxInfo::createMass(dMass * mass, float massVal)
{
dMassSetBoxTotal(mass, massVal, lx, ly, lz);
}
