/*** 車輪の生成 ***/
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]);
}
}
}
