void TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
dMass totalMass;
dBodyGetMass( m_BodyID, &totalMass );
if ( m_GeomIDs.size() == 0 )
{
dMassSetZero( &totalMass );
}
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyToGeomTransform, R, P );
dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
dGeomID encapsulatedGeom = 0;
dMass currMass;
dMassSetZero( &currMass );
encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );
dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
dMassRotate( &currMass, R );
//dMassTranslate(&currMass,P[0],P[1],P[2]);
dMassAdd( &totalMass, &currMass );
dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
dGeomSetRotation( encapsulatedGeom, R );
dGeomTransformSetCleanup( geomTransform, 1 );
dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
dGeomSetBody( geomTransform, m_BodyID );
m_GeomIDs.push_back( geomTransform );
dBodySetMass( m_BodyID, &totalMass );
}
// Create a ball and a pole
void createBallandPole() {
dMass m1;
dReal x0 = 0.0, y0 = 0.0, z0 = 2.5;
// ball
ball.radius = 0.2;
ball.mass = 1.0;
ball.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetSphereTotal(&m1, ball.mass, ball.radius);
dBodySetMass(ball.body, &m1);
dBodySetPosition(ball.body, x0, y0, z0);
ball.geom = dCreateSphere(space, ball.radius);
dGeomSetBody(ball.geom, ball.body);
// pole
pole.radius = 0.025;
pole.length = 1.0;
pole.mass = 1.0;
pole.body = dBodyCreate(world);
dMassSetZero(&m1);
dMassSetCapsule(&m1, pole.mass, 3, pole.radius, pole.length);
dBodySetMass(pole.body, &m1);
dBodySetPosition(pole.body, x0, y0, z0 - 0.5 * pole.length);
pole.geom = dCreateCCylinder(space, pole.radius, pole.length);
dGeomSetBody(pole.geom, pole.body);
// hinge joint
joint = dJointCreateHinge(world, 0);
dJointAttach(joint, ball.body, pole.body);
dJointSetHingeAnchor(joint, x0, y0, z0 - ball.radius);
dJointSetHingeAxis(joint, 1, 0, 0);
}
//Mass
void PhysicsBody::resetMass()
{
dMass mass;
getMassStruct(mass);
dMassSetZero(&mass);
setMassStruct(mass);
}
void createPart(const dWorldID & _world , dPart & part, dReal * pos3){
dMass mass;
part.body = dBodyCreate(_world);
dBodySetPosition(part.body, pos3[0] , pos3[1], pos3[2]); // Set a position
dMassSetZero(&mass); // Set mass parameter to zero
dMassSetCapsuleTotal(&mass,part.mass,3,part.radius, part.length); // Calculate mass parameter
dBodySetMass(part.body, &mass); // Set mass
}
/*** ロボットアームの生成 ***/
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]);
}
}
/*** 車輪の生成 ***/
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 SBoxParts::set(dWorldID w, dSpaceID space)
{
Size &sz = m_cmpnt.size();
/*
const dReal hx = sz.x();
const dReal hy = sz.y();
const dReal hz = sz.z();
*/
dReal hx = sz.x();
dReal hy = sz.y();
dReal hz = sz.z();
// konao
DUMP(("[SBoxParts::set] ODE geom created (hx, hy, hz)=(%f, %f, %f) [%s:%d]\n", hx, hy, hz, __FILE__, __LINE__));
if (hz == 0) hz = 0.001;
if (hy == 0) hy = 0.001;
if (hx == 0) hx = 0.001;
dGeomID geom = dCreateBox(0, hx, hy, hz);
m_odeobj = ODEObjectContainer::getInstance()->createODEObj
(
w,
geom,
0.9,
0.01,
0.5,
0.5,
0.8,
0.001,
0.0
);
dBodyID body = m_odeobj->body();
dMass m;
dMassSetZero(&m);
// x-axis and z-axis is swapped between ODE/SIGVerse
dMassSetBox(&m, DENSITY, hz, hy, hx); //TODO: mass of cube should be configurable
dMassAdjust(&m, m_mass);
dBodySetMass(body, &m);
// Gap between ODE shape and body
dGeomSetOffsetPosition(geom, m_posx, m_posy, m_posz);
// Initial orientation
dReal offq[4] = {m_inirot.qw(), m_inirot.qx(), m_inirot.qy(), m_inirot.qz()};
dGeomSetOffsetQuaternion(geom, offq);
//dMassAdjust(&m, 1.0);
m_rot.setQuaternion(1.0, 0.0, 0.0, 0.0);
dSpaceAdd(space, geom);
dBodySetData(body, this);
}
void PhySphere::SetMass(float mass, float radius)
{
if (mBodyId != NULL)
{
dMass m;
dMassSetZero(&m);
dMassSetSphereTotal(&m, mass, radius);
dBodySetMass(mOdeBody, &m);
}
}
void box::set_mass(double mass)
{
if(body_id)
{
dMass dmass;
dMassSetZero(&dmass);
dMassSetBoxTotal (&dmass, mass, size[0], size[1], size[2]);
dBodySetMass (body_id, &dmass);
material.mass = mass;
}
}
void sphere::set_mass(double mass)
{
if(body_id)
{
dMass dmass;
dMassSetZero(&dmass);
dMassSetSphereTotal(&dmass,mass,radius);
dBodySetMass (body_id, &dmass);
material.mass = mass;
}
}
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 capsule::set_mass(double mass)
{
if(body_id)
{
dMass dmass;
dMassSetZero(&dmass);
dMassSetSphereTotal(&dmass,mass,radius);
dMassSetCapsuleTotal(&dmass,mass,3,radius,length);
dBodySetMass (body_id, &dmass);
material.mass = mass;
}
}
int main (int argc, char **argv)
{
// set for drawing
dsFunctions fn;
fn.version = DS_VERSION;
fn.start = &start;
fn.step = &simLoop;
fn.command = NULL;
fn.stop = NULL;
fn.path_to_textures = "../textures";
dInitODE(); // init ODE
world = dWorldCreate(); // create a dynamic world
dWorldSetGravity(world,0,0,-0.1);
dMass m; // a parameter for mass
dMassSetZero (&m); // initialize the parameter
//@a sphere
sphere.body = dBodyCreate (world); // create a rigid body
dReal radius = 0.5; // radius [m]
dMassSetSphere (&m,DENSITY,radius); // calculate a mass parameter for a sphere
dBodySetMass (sphere.body,&m); // set the mass parameter to the body
dBodySetPosition (sphere.body,0,1, 1); // set the position of the body
//@a box
box.body = dBodyCreate (world);
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
dBodySetMass (box.body,&m);
dBodySetPosition (box.body,0,2,1);
// a capsule
capsule.body = dBodyCreate (world);
dMassSetCapsule(&m,DENSITY,3,radius,length);
dBodySetMass (capsule.body,&m);
dBodySetPosition (capsule.body,0,4,1);
// a cylinder
cylinder.body = dBodyCreate (world);
dMassSetCylinder(&m,DENSITY,3,radius,length);
dBodySetMass (cylinder.body,&m);
dBodySetPosition (cylinder.body,0,3,1);
// do the simulation
dsSimulationLoop (argc,argv,960,480,&fn);
dWorldDestroy (world); // destroy the world
dCloseODE(); // close ODE
return 0;
}
void dMassSetBoxTotal (dMass *m, dReal total_mass,
dReal lx, dReal ly, dReal lz)
{
dAASSERT (m);
dMassSetZero (m);
m->mass = total_mass;
m->_I(0,0) = total_mass/REAL(12.0) * (ly*ly + lz*lz);
m->_I(1,1) = total_mass/REAL(12.0) * (lx*lx + lz*lz);
m->_I(2,2) = total_mass/REAL(12.0) * (lx*lx + ly*ly);
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dMassSetSphereTotal (dMass *m, dReal total_mass, dReal radius)
{
dAASSERT (m);
dMassSetZero (m);
m->mass = total_mass;
dReal II = REAL(0.4) * total_mass * radius*radius;
m->_I(0,0) = II;
m->_I(1,1) = II;
m->_I(2,2) = II;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
dMass KinematicMass::getODEMass(arma::mat44 coordinateFrame) const
{
dMass mass;
arma::colvec4 helper = arma::zeros(4);
helper(3) = 1.;
helper.rows(0, 2) = m_position;
helper = coordinateFrame * helper;
dMassSetZero(&mass);
dMassSetSphereTotal(&mass, m_massGrams / 1000., 1);
dMassTranslate(&mass, helper(0), helper(1), helper(2));
return mass;
}
//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);
}
}
CShape::CShape(EType ShapeType):
Type(ShapeType),
pRigidBody(NULL),
pEntity(NULL),
CollCount(0),
IsHorizColl(false),
CatBits(Dynamic),
CollBits(All),
ODEGeomID(NULL),
ODESpaceID(NULL),
MaterialType(InvalidMaterial)
{
//MaterialType = CMaterialTable::StringToMaterialType("Metal"); //???invalid material?
dMassSetZero(&ODEMass); //!!!need to calculate mass somewhere!
}
//set mass and radius by available total mass
void ODE_Particle::setMassTotal(dReal total_mass, dReal rad)
{
dMass m;
dMassSetZero(&m);
if (getShapeType() == dSphereClass)
{
dMassSetSphereTotal(&m,total_mass,rad); // set a given mass to sphere
dBodySetMass(body,&m);
}
else
{
printf("ERROR in ODE_Particle.cpp: Setting totalMass for non spherical object");
exit(0);
}
}
//set mass by radius and density
void ODE_Particle::setMass(dReal density, dReal rad)
{
dMass m;
dMassSetZero(&m);
if (getShapeType() == dSphereClass)
{
dMassSetSphere (&m,density,rad); // calculate a mass for a sphere
dBodySetMass(body,&m);
}
else
{
printf("ERROR in ODE_Particle.cpp: Setting Mass using density for non spherical object");
exit(0);
}
}
CDynamics3DEntity::CDynamics3DEntity(CDynamics3DEngine& c_engine,
CEmbodiedEntity& c_entity) :
CPhysicsEngineEntity(c_entity),
m_cEngine(c_engine),
m_tEngineWorld(c_engine.GetWorldID()),
m_tEngineSpace(c_engine.GetSpaceID()),
m_tBody(dBodyCreate(m_tEngineWorld)),
m_tEntitySpace(dSimpleSpaceCreate(m_tEngineSpace)) {
/* Zero the body mass */
dMassSetZero(&m_tMass);
/*
* Set the level of the body space
* Necessary for dSpaceCollide2 to work properly
*/
dSpaceSetSublevel(m_tEngineSpace, 1);
}
//set mass by sides and density for box objects
void ODE_Particle::setMass(dReal density, dReal side1,dReal side2,dReal side3)
{
dMass m;
dMassSetZero(&m);
if (getShapeType() == dBoxClass)
{
dMassSetBox (&m,density,side1,side2,side3);
dBodySetMass(body,&m);
}
else
{
printf("ERROR in ODE_Particle.cpp: Setting Mass using density 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);
}
/*** ボールの生成 ***/
static void makeBall()
{
dMass mass;
ball.m = 0.45;
ball.r = 0.11;
ball.x = 1.0;
ball.y = 0.0;
ball.z = 0.14 + offset_z;
ball.body = dBodyCreate(world);
dMassSetZero(&mass);
dMassSetSphereTotal(&mass,ball.m,ball.r);
dBodySetMass(ball.body,&mass);
ball.geom = dCreateSphere(space,ball.r);
dGeomSetBody(ball.geom,ball.body);
dBodySetPosition(ball.body,ball.x, ball.y, ball.z);
}
void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction,
dReal radius, dReal length)
{
dReal r2,I;
dAASSERT (m);
dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
dMassSetZero (m);
r2 = radius*radius;
m->mass = total_mass;
I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length);
m->_I(0,0) = I;
m->_I(1,1) = I;
m->_I(2,2) = I;
m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
dMass KinematicNodeDummy::getODEMassToAttachToParent(arma::mat44 coordinateFrame) const
{
dMass compositeMass;
dMassSetZero(&compositeMass);
for (KinematicMass const& mass : m_masses)
{
dMass componentMass = mass.getODEMass(coordinateFrame);
dMassAdd(&compositeMass, &componentMass);
}
for (KinematicNode *child : m_children)
{
arma::mat44 childFrame = coordinateFrame * child->getForwardMatrix();
dMass componentMass = child->getODEMassToAttachToParent(childFrame);
dMassAdd(&compositeMass, &componentMass);
}
return compositeMass;
}
void computeMassParams (dMass *m, dReal q[NUM][3], dReal pm[NUM])
{
int i,j;
dMassSetZero (m);
for (i=0; i<NUM; i++) {
m->mass += pm[i];
for (j=0; j<3; j++) m->c[j] += pm[i]*q[i][j];
m->_I(0,0) += pm[i]*(q[i][1]*q[i][1] + q[i][2]*q[i][2]);
m->_I(1,1) += pm[i]*(q[i][0]*q[i][0] + q[i][2]*q[i][2]);
m->_I(2,2) += pm[i]*(q[i][0]*q[i][0] + q[i][1]*q[i][1]);
m->_I(0,1) -= pm[i]*(q[i][0]*q[i][1]);
m->_I(0,2) -= pm[i]*(q[i][0]*q[i][2]);
m->_I(1,2) -= pm[i]*(q[i][1]*q[i][2]);
}
for (j=0; j<3; j++) m->c[j] /= m->mass;
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
}
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);
}
