const dMass& Mass::createMass() { if(!created) { assembleMass(); for(std::list<SimObject*>::const_iterator iter = children.begin(), end = children.end(); iter != end; ++iter) { Mass* childMassDesc = dynamic_cast<Mass*>(*iter); ASSERT(childMassDesc); const dMass& childMass = childMassDesc->createMass(); if(childMassDesc->translation || childMassDesc->rotation) { dMass shiftedChildMass = childMass; if(childMassDesc->rotation) { dMatrix3 matrix; ODETools::convertMatrix(*childMassDesc->rotation, matrix); dMassRotate(&shiftedChildMass, matrix); } if(childMassDesc->translation) dMassTranslate(&shiftedChildMass, childMassDesc->translation->x, childMassDesc->translation->y, childMassDesc->translation->z); dMassAdd(&mass, &shiftedChildMass); } else dMassAdd(&mass, &childMass); } created = true; } return mass; }
void CODEGeom::get_mass(dMass& m,const Fvector& ref_point) { get_mass(m); Fvector l; l.sub(local_center(),ref_point); dMassTranslate(&m,l.x,l.y,l.z); }
void PhysicsBody::translateMass( const Vec3f& t) { dMass mass; getMassStruct(mass); dMassTranslate(&mass, t.x(), t.y(), t.z() ); setMassStruct(mass); }
void TSRODERigidBody::AddSphereGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius, 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 = dCreateSphere( 0, _fRadius ); dMassSetSphere( &currMass, _fDensity, _fRadius ); 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 ); }
void CODEGeom::get_mass(dMass& m,const Fvector& ref_point, float density) { get_mass(m); dMassAdjust(&m,density*volume()); Fvector l; l.sub(local_center(),ref_point); dMassTranslate(&m,l.x,l.y,l.z); }
// Transform the own dMass structure by the own Transform matrix. void CShape::TransformMass() { dMatrix3 ODERotation; CPhysicsServer::Matrix44ToOde(Transform, ODERotation); dMassRotate(&ODEMass, ODERotation); const vector3& Pos = Transform.pos_component(); dMassTranslate(&ODEMass, Pos.x, Pos.y, Pos.z); }
void end_phys_mass(dMass *mass, dBodyID body, float center[3]) { /* Translate the center of mass to the origin. */ center[0] = (float) mass->c[0]; center[1] = (float) mass->c[1]; center[2] = (float) mass->c[2]; dMassTranslate(mass, -mass->c[0], -mass->c[1], -mass->c[2]); dBodySetMass(body, mass); }
void PhysicsObject::attachObject(boost::shared_ptr<PhysicsObject> po, const v3& position, const qv4& orientation) { po->mBodyOffset = position; GeomMapT::const_iterator it = po->mGeometry.begin(); for (; it != po->mGeometry.end(); ++it) { // Calculate new relative position dGeomID geom = it->second.geomId; const dReal* offset = dGeomGetOffsetPosition(geom); v3 newPos(offset); newPos += position; // Attach dGeomSetBody(geom, mOdeBody); dGeomSetOffsetPosition(geom, newPos.x, newPos.y, newPos.z); dSpaceRemove(po->mSpaceId, geom); dSpaceAdd(mSpaceId, geom); } // add the two masses dMass otherMass; dBodyGetMass(po->mOdeBody, &otherMass); // dbglog << "OtherMass: " << otherMass.mass; // dbglog << "OtherCenter: " << odeVectorOut(otherMass.c); // dbglog << "OtherInertia: " << odeMatrixOut(otherMass.I); dBodyGetMass(mOdeBody, &mOriginalMass); // dbglog << "OwnMass: " << mOriginalMass.mass; // dbglog << "OwnCenter: " << odeVectorOut(mOriginalMass.c); // dbglog << "OwnInertia: " << odeMatrixOut(mOriginalMass.I); dMassAdd(&mOriginalMass, &otherMass); dMassTranslate(&mOriginalMass, -mOriginalMass.c[0], -mOriginalMass.c[1], -mOriginalMass.c[2]); dBodySetMass(mOdeBody, &mOriginalMass); // dbglog << "NewMass: " << mOriginalMass.mass; // dbglog << "NewCenter: " << odeVectorOut(mOriginalMass.c); // dbglog << "NewInertia: " << odeMatrixOut(mOriginalMass.I); // Disable old body dBodyDisable(po->mOdeBody); notifyControlAboutChangeInMass(); }
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; }
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); } }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; bool setBody = false; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'v' /* || cmd == 'l' */) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+1); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } /* // cylinder option not yet implemented else if (cmd == 'l') { sides[1] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } */ else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x') { setBody = true; // start accumulating masses for the composite geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for composite geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule(space,radius,length); dMassSetCapsule(&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate(&m2,drot[k]); dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd(&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody(obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition(obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass(obj[i].body,&m); } else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex(space, planes, planecount, points, pointcount, polygons); } if (!setBody) { // avoid calling for composite geometries for (k=0; k < GPB; k++) if (obj[i].geom[k]) dGeomSetBody(obj[i].geom[k],obj[i].body); dBodySetMass(obj[i].body,&m); } } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; bool setBody = false; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' || cmd == 'v') { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); obj[i].geom[0] = dCreateConvex(space, planes, planecount, points, pointcount, polygons); } else if (cmd == 'y') { sides[1] *= 0.5; dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'm') { dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); dGeomTriMeshDataPreprocess2(new_tmdata, (1U << dTRIDATAPREPROCESS_BUILD_FACE_ANGLES), NULL); obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); // remember the mesh's dTriMeshDataID on its userdata for convenience. dGeomSetData(obj[i].geom[0], new_tmdata); dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); } else if (cmd == 'x') { setBody = true; // start accumulating masses for the composite geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for composite geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox(space,sides[0],sides[1],sides[2]); dMassSetBox(&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule(space,radius,length); dMassSetCapsule(&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle(drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate(&m2,drot[k]); dMassTranslate(&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd(&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody(obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition(obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate(&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass(obj[i].body,&m); } if (!setBody) { // avoid calling for composite geometries for (k=0; k < GPB; k++) if (obj[i].geom[k]) dGeomSetBody(obj[i].geom[k],obj[i].body); dBodySetMass(obj[i].body,&m); } } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
//! 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) { }
static void command (int cmd) { size_t i; int j,k; dReal sides[3]; dMass m; int setBody; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'y' || cmd == 'v') { setBody = 0; if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRSetIdentity (R); //dRFromAxisAndAngle (R,0,0,1,/*dRandReal()*10.0-5.0*/0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*) i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } //<---- Convex Object else if (cmd == 'v') { dMassSetBox (&m,DENSITY,0.25,0.25,0.25); #if 0 obj[i].geom[0] = dCreateConvex (space, planes, planecount, points, pointcount, polygons); #else obj[i].geom[0] = dCreateConvex (space, Sphere_planes, Sphere_planecount, Sphere_points, Sphere_pointcount, Sphere_polygons); #endif } //----> Convex Object else if (cmd == 'y') { dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x' && USE_GEOM_OFFSET) { setBody = 1; // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); dReal dpos[GPB][3]; // delta-positions for encapsulated geometries dMatrix3 drot[GPB]; // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { if (k==0) { dReal radius = dRandReal()*0.25+0.05; obj[i].geom[k] = dCreateSphere (space,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { obj[i].geom[k] = dCreateBox (space,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; obj[i].geom[k] = dCreateCapsule (space,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dRFromAxisAndAngle (drot[k],dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dMassRotate (&m2,drot[k]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd (&m,&m2); } for (k=0; k<GPB; k++) { dGeomSetBody (obj[i].geom[k],obj[i].body); dGeomSetOffsetPosition (obj[i].geom[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); dGeomSetOffsetRotation(obj[i].geom[k], drot[k]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); dBodySetMass (obj[i].body,&m); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCapsule (0,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // Translation *after* rotation dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<GPB; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } if (!setBody) for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } else if (cmd == '1') { write_world = 1; } else if (cmd == 'p'&& selected >= 0) { const dReal* pos = dGeomGetPosition(obj[selected].geom[0]); const dReal* rot = dGeomGetRotation(obj[selected].geom[0]); printf("POSITION:\n\t[%f,%f,%f]\n\n",pos[0],pos[1],pos[2]); printf("ROTATION:\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\t[%f,%f,%f,%f]\n\n", rot[0],rot[1],rot[2],rot[3], rot[4],rot[5],rot[6],rot[7], rot[8],rot[9],rot[10],rot[11]); } else if (cmd == 'f' && selected >= 0 && selected < num) { if (dBodyIsEnabled(obj[selected].body)) doFeedback = 1; } }
// called when a key pressed static void command( int cmd ) { int i,k; dReal sides[3]; dMass m; cmd = locase( cmd ); if ( cmd == 'v' || cmd == 'b' || cmd == 'c' || cmd == 's' ) { if ( num < NUM ) { i = num; num++; } else { i = nextobj; nextobj++; if ( nextobj >= num ) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy( obj[i].body ); for ( k=0; k < GPB; k++ ) { if ( obj[i].geom[k] ) dGeomDestroy( obj[i].geom[k] ); } memset( &obj[i],0,sizeof( obj[i] ) ); } obj[i].body = dBodyCreate( world ); for ( k=0; k<3; k++ ) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if ( random_pos ) { dBodySetPosition( obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3 ); dRFromAxisAndAngle( R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0 ); } else { dReal maxheight = 0; for ( k=0; k<num; k++ ) { const dReal *pos = dBodyGetPosition( obj[k].body ); if ( pos[2] > maxheight ) maxheight = pos[2]; } dBodySetPosition( obj[i].body, 0,0,maxheight+1 ); dRFromAxisAndAngle( R,0,0,1,dRandReal()*10.0-5.0 ); } dBodySetRotation( obj[i].body,R ); dBodySetData( obj[i].body,( void* )( size_t )i ); if ( cmd == 'b' ) { dMassSetBox( &m,DENSITY,sides[0],sides[1],sides[2] ); obj[i].geom[0] = dCreateBox( space,sides[0],sides[1],sides[2] ); } else if ( cmd == 'c' ) { sides[0] *= 0.5; dMassSetCapsule( &m,DENSITY,3,sides[0],sides[1] ); obj[i].geom[0] = dCreateCapsule( space,sides[0],sides[1] ); } else if ( cmd == 's' ) { sides[0] *= 0.5; dMassSetSphere( &m,DENSITY,sides[0] ); obj[i].geom[0] = dCreateSphere( space,sides[0] ); } else if ( cmd == 'v' ) { obj[i].geom[0] = dCreateConvex( space, convexBunnyPlanes, convexBunnyPlaneCount, convexBunnyPoints, convexBunnyPointCount, convexBunnyPolygons ); /// Use equivalent TriMesh to set mass dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle( new_tmdata, &Vertices[0], 3 * sizeof( float ), VertexCount, ( dTriIndex* )&Indices[0], IndexCount, 3 * sizeof( dTriIndex ) ); dGeomID triMesh = dCreateTriMesh( 0, new_tmdata, 0, 0, 0 ); dMassSetTrimesh( &m, DENSITY, triMesh ); dGeomDestroy( triMesh ); dGeomTriMeshDataDestroy( new_tmdata ); printf( "mass at %f %f %f\n", m.c[0], m.c[1], m.c[2] ); dGeomSetPosition( obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2] ); dMassTranslate( &m, -m.c[0], -m.c[1], -m.c[2] ); } for ( k=0; k < GPB; k++ ) { if ( obj[i].geom[k] ) dGeomSetBody( obj[i].geom[k],obj[i].body ); } dBodySetMass( obj[i].body,&m ); } if ( cmd == ' ' ) { selected++; if ( selected >= num ) selected = 0; if ( selected < 0 ) selected = 0; } else if ( cmd == 'd' && selected >= 0 && selected < num ) { dBodyDisable( obj[selected].body ); } else if ( cmd == 'e' && selected >= 0 && selected < num ) { dBodyEnable( obj[selected].body ); } else if ( cmd == 'a' ) { show_aabb ^= 1; } else if ( cmd == 't' ) { show_contacts ^= 1; } else if ( cmd == 'r' ) { random_pos ^= 1; } }
static void command (int cmd) { size_t i; int j,k; dReal sides[3]; dMass m; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' /* || cmd == 'l' */) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+2); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*) i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCCylinder (space,sides[0],sides[1]); } /* // cylinder option not yet implemented else if (cmd == 'l') { sides[1] *= 0.5; dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } */ else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCCylinder (0,radius,length); dMassSetCappedCylinder (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<2; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } else if (cmd == '1') { write_world = 1; } }
/* * dMassSetTrimesh, implementation by Gero Mueller. * Based on Brian Mirtich, "Fast and Accurate Computation of * Polyhedral Mass Properties," journal of graphics tools, volume 1, * number 2, 1996. */ void dMassSetTrimesh( dMass *m, dReal density, dGeomID g ) { dAASSERT (m); dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh"); dMassSetZero (m); #if dTRIMESH_ENABLED dxTriMesh *TriMesh = (dxTriMesh *)g; unsigned int triangles = FetchTriangleCount( TriMesh ); dReal nx, ny, nz; unsigned int i, A, B, C; // face integrals dReal Fa, Fb, Fc, Faa, Fbb, Fcc, Faaa, Fbbb, Fccc, Faab, Fbbc, Fcca; // projection integrals dReal P1, Pa, Pb, Paa, Pab, Pbb, Paaa, Paab, Pabb, Pbbb; dReal T0 = 0; dReal T1[3] = {0., 0., 0.}; dReal T2[3] = {0., 0., 0.}; dReal TP[3] = {0., 0., 0.}; for( i = 0; i < triangles; i++ ) { dVector3 v[3]; FetchTransformedTriangle( TriMesh, i, v); dVector3 n, a, b; dSubtractVectors3( a, v[1], v[0] ); dSubtractVectors3( b, v[2], v[0] ); dCalcVectorCross3( n, b, a ); nx = fabs(n[0]); ny = fabs(n[1]); nz = fabs(n[2]); if( nx > ny && nx > nz ) C = 0; else C = (ny > nz) ? 1 : 2; // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (n[C] != REAL(0.0)) { A = (C + 1) % 3; B = (A + 1) % 3; // calculate face integrals { dReal w; dReal k1, k2, k3, k4; //compProjectionIntegrals(f); { dReal a0=0, a1=0, da; dReal b0=0, b1=0, db; dReal a0_2, a0_3, a0_4, b0_2, b0_3, b0_4; dReal a1_2, a1_3, b1_2, b1_3; dReal C1, Ca, Caa, Caaa, Cb, Cbb, Cbbb; dReal Cab, Kab, Caab, Kaab, Cabb, Kabb; P1 = Pa = Pb = Paa = Pab = Pbb = Paaa = Paab = Pabb = Pbbb = 0.0; for( int j = 0; j < 3; j++) { switch(j) { case 0: a0 = v[0][A]; b0 = v[0][B]; a1 = v[1][A]; b1 = v[1][B]; break; case 1: a0 = v[1][A]; b0 = v[1][B]; a1 = v[2][A]; b1 = v[2][B]; break; case 2: a0 = v[2][A]; b0 = v[2][B]; a1 = v[0][A]; b1 = v[0][B]; break; } da = a1 - a0; db = b1 - b0; a0_2 = a0 * a0; a0_3 = a0_2 * a0; a0_4 = a0_3 * a0; b0_2 = b0 * b0; b0_3 = b0_2 * b0; b0_4 = b0_3 * b0; a1_2 = a1 * a1; a1_3 = a1_2 * a1; b1_2 = b1 * b1; b1_3 = b1_2 * b1; C1 = a1 + a0; Ca = a1*C1 + a0_2; Caa = a1*Ca + a0_3; Caaa = a1*Caa + a0_4; Cb = b1*(b1 + b0) + b0_2; Cbb = b1*Cb + b0_3; Cbbb = b1*Cbb + b0_4; Cab = 3*a1_2 + 2*a1*a0 + a0_2; Kab = a1_2 + 2*a1*a0 + 3*a0_2; Caab = a0*Cab + 4*a1_3; Kaab = a1*Kab + 4*a0_3; Cabb = 4*b1_3 + 3*b1_2*b0 + 2*b1*b0_2 + b0_3; Kabb = b1_3 + 2*b1_2*b0 + 3*b1*b0_2 + 4*b0_3; P1 += db*C1; Pa += db*Ca; Paa += db*Caa; Paaa += db*Caaa; Pb += da*Cb; Pbb += da*Cbb; Pbbb += da*Cbbb; Pab += db*(b1*Cab + b0*Kab); Paab += db*(b1*Caab + b0*Kaab); Pabb += da*(a1*Cabb + a0*Kabb); } P1 /= 2.0; Pa /= 6.0; Paa /= 12.0; Paaa /= 20.0; Pb /= -6.0; Pbb /= -12.0; Pbbb /= -20.0; Pab /= 24.0; Paab /= 60.0; Pabb /= -60.0; } w = - dCalcVectorDot3(n, v[0]); k1 = 1 / n[C]; k2 = k1 * k1; k3 = k2 * k1; k4 = k3 * k1; Fa = k1 * Pa; Fb = k1 * Pb; Fc = -k2 * (n[A]*Pa + n[B]*Pb + w*P1); Faa = k1 * Paa; Fbb = k1 * Pbb; Fcc = k3 * (SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb + w*(2*(n[A]*Pa + n[B]*Pb) + w*P1)); Faaa = k1 * Paaa; Fbbb = k1 * Pbbb; Fccc = -k4 * (CUBE(n[A])*Paaa + 3*SQR(n[A])*n[B]*Paab + 3*n[A]*SQR(n[B])*Pabb + CUBE(n[B])*Pbbb + 3*w*(SQR(n[A])*Paa + 2*n[A]*n[B]*Pab + SQR(n[B])*Pbb) + w*w*(3*(n[A]*Pa + n[B]*Pb) + w*P1)); Faab = k1 * Paab; Fbbc = -k2 * (n[A]*Pabb + n[B]*Pbbb + w*Pbb); Fcca = k3 * (SQR(n[A])*Paaa + 2*n[A]*n[B]*Paab + SQR(n[B])*Pabb + w*(2*(n[A]*Paa + n[B]*Pab) + w*Pa)); } T0 += n[0] * ((A == 0) ? Fa : ((B == 0) ? Fb : Fc)); T1[A] += n[A] * Faa; T1[B] += n[B] * Fbb; T1[C] += n[C] * Fcc; T2[A] += n[A] * Faaa; T2[B] += n[B] * Fbbb; T2[C] += n[C] * Fccc; TP[A] += n[A] * Faab; TP[B] += n[B] * Fbbc; TP[C] += n[C] * Fcca; } } T1[0] /= 2; T1[1] /= 2; T1[2] /= 2; T2[0] /= 3; T2[1] /= 3; T2[2] /= 3; TP[0] /= 2; TP[1] /= 2; TP[2] /= 2; m->mass = density * T0; m->_I(0,0) = density * (T2[1] + T2[2]); m->_I(1,1) = density * (T2[2] + T2[0]); m->_I(2,2) = density * (T2[0] + T2[1]); m->_I(0,1) = - density * TP[0]; m->_I(1,0) = - density * TP[0]; m->_I(2,1) = - density * TP[1]; m->_I(1,2) = - density * TP[1]; m->_I(2,0) = - density * TP[2]; m->_I(0,2) = - density * TP[2]; // Added to address SF bug 1729095 dMassTranslate( m, T1[0] / T0, T1[1] / T0, T1[2] / T0 ); # ifndef dNODEBUG dMassCheck (m); # endif #endif // dTRIMESH_ENABLED }
void testMassFunctions() { dMass m; int i,j; dReal q[NUMP][3]; // particle positions dReal pm[NUMP]; // particle masses dMass m1,m2; dMatrix3 R; HEADER; printf ("\t"); dMassSetZero (&m); TRAP_MESSAGE (dMassSetParameters (&m,10, 0,0,0, 1,2,3, 4,5,6), printf (" FAILED (1)\n"), printf (" passed (1)\n")); printf ("\t"); dMassSetZero (&m); TRAP_MESSAGE (dMassSetParameters (&m,10, 0.1,0.2,0.15, 3,5,14, 3.1,3.2,4), printf ("passed (2)\n") , printf (" FAILED (2)\n")); if (m.mass==10 && m.c[0]==REAL(0.1) && m.c[1]==REAL(0.2) && m.c[2]==REAL(0.15) && m._I(0,0)==3 && m._I(1,1)==5 && m._I(2,2)==14 && m._I(0,1)==REAL(3.1) && m._I(0,2)==REAL(3.2) && m._I(1,2)==4 && m._I(1,0)==REAL(3.1) && m._I(2,0)==REAL(3.2) && m._I(2,1)==4) printf ("\tpassed (3)\n"); else printf ("\tFAILED (3)\n"); dMassSetZero (&m); dMassSetSphere (&m,1.4, 0.86); if (cmp(m.mass,3.73002719949386) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && cmp(m._I(0,0),1.10349124669826) && cmp(m._I(1,1),1.10349124669826) && cmp(m._I(2,2),1.10349124669826) && m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) printf ("\tpassed (4)\n"); else printf ("\tFAILED (4)\n"); dMassSetZero (&m); dMassSetCapsule (&m,1.3,1,0.76,1.53); if (cmp(m.mass,5.99961928996029) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && cmp(m._I(0,0),1.59461986077384) && cmp(m._I(1,1),4.21878433864904) && cmp(m._I(2,2),4.21878433864904) && m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) printf ("\tpassed (5)\n"); else printf ("\tFAILED (5)\n"); dMassSetZero (&m); dMassSetBox (&m,0.27,3,4,5); if (cmp(m.mass,16.2) && m.c[0]==0 && m.c[1]==0 && m.c[2]==0 && cmp(m._I(0,0),55.35) && cmp(m._I(1,1),45.9) && cmp(m._I(2,2),33.75) && m._I(0,1)==0 && m._I(0,2)==0 && m._I(1,2)==0 && m._I(1,0)==0 && m._I(2,0)==0 && m._I(2,1)==0) printf ("\tpassed (6)\n"); else printf ("\tFAILED (6)\n"); // test dMassAdjust? // make random particles and compute the mass, COM and inertia, then // translate and repeat. for (i=0; i<NUMP; i++) { pm[i] = dRandReal()+0.5; for (j=0; j<3; j++) { q[i][j] = 2.0*(dRandReal()-0.5); } } computeMassParams (&m1,q,pm); memcpy (&m2,&m1,sizeof(dMass)); dMassTranslate (&m2,1,2,-3); for (i=0; i<NUMP; i++) { q[i][0] += 1; q[i][1] += 2; q[i][2] -= 3; } computeMassParams (&m1,q,pm); compareMassParams (&m1,&m2,"7"); // rotate the masses _R(0,0) = -0.87919618797635; _R(0,1) = 0.15278881840384; _R(0,2) = -0.45129772879842; _R(1,0) = -0.47307856232664; _R(1,1) = -0.39258064912909; _R(1,2) = 0.78871864932708; _R(2,0) = -0.05666336483842; _R(2,1) = 0.90693771059546; _R(2,2) = 0.41743652473765; dMassRotate (&m2,R); for (i=0; i<NUMP; i++) { dReal a[3]; dMultiply0 (a,&_R(0,0),&q[i][0],3,3,1); q[i][0] = a[0]; q[i][1] = a[1]; q[i][2] = a[2]; } computeMassParams (&m1,q,pm); compareMassParams (&m1,&m2,"8"); }
dBodyID KinematicNodeDummy::attachToODE(PhysicsEnvironment *environment, dSpaceID visualSpaceID, dSpaceID collisionSpaceID, arma::mat44 parentsCoordinateFrame) { // move coordinate frame to current node: arma::mat44 coordinateFrame = parentsCoordinateFrame * forwardMatrix; dWorldID worldID = environment->getWorldID(); if (nullptr == m_parent) { // the body must coincide with the center of mass of the equivalent mass of all dummy children and this nodes masses arma::mat44 frame = arma::eye(4, 4); dMass equivMass = this->getODEMass(frame); for (KinematicNode *child : m_children) { arma::mat44 childFrame = frame * child->getForwardMatrix(); dMass childMass = child->getODEMassToAttachToParent(childFrame); if (childMass.mass > 0.) { dMassAdd(&equivMass, &childMass); } } m_odeNodeBodyOffset = arma::eye(4, 4); m_odeNodeBodyOffset(0, 3) = equivMass.c[0]; m_odeNodeBodyOffset(1, 3) = equivMass.c[1]; m_odeNodeBodyOffset(2, 3) = equivMass.c[2]; dMassTranslate(&equivMass, -m_odeNodeBodyOffset(0, 3), -m_odeNodeBodyOffset(1, 3), -m_odeNodeBodyOffset(2, 3)); // create a body m_odeBody = dBodyCreate(worldID); arma::mat44 globalSystemBodyOffset = coordinateFrame * m_odeNodeBodyOffset; dMatrix3 bodyRotation; dVector3 bodyPosition; odeUtils::getRotationMatrixAsDMat(globalSystemBodyOffset, bodyRotation); odeUtils::getPositionAsDVec(globalSystemBodyOffset, bodyPosition); dBodySetPosition(m_odeBody, bodyPosition[0], bodyPosition[1], bodyPosition[2]); dBodySetRotation(m_odeBody, bodyRotation); if (equivMass.mass > 0.) { dBodySetMass(m_odeBody, &equivMass); } else { dBodyGetMass(m_odeBody, &equivMass); equivMass.mass = std::numeric_limits<double>::min(); dBodySetMass(m_odeBody, &equivMass); } } else { // setup the nodeBodyOffset based on the parent's odeBody dBodyID body = this->getODEBody(); const dReal *bodyPosition = dBodyGetPosition(body); const dReal *bodyRotation = dBodyGetRotation(body); arma::mat44 bodyFrame = odeUtils::getMatFromDMatAndDVec(bodyRotation, bodyPosition); arma::mat44 helperRotation = arma::eye(4, 4); helperRotation.submat(0, 0, 2, 2) = coordinateFrame.submat(0, 0, 2, 2); m_odeNodeBodyOffset = coordinateFrame.i() * bodyFrame; } // create a "box" to visualize the node dGeomID geom = dCreateSphere(visualSpaceID, 0.005); dMatrix3 bodyRotation; dVector3 bodyPosition; // the visuals are defined as "local" coordinates but here we need to incorporate the body offset arma::mat44 visualsFrame = m_odeNodeBodyOffset.i(); odeUtils::getRotationMatrixAsDMat(visualsFrame, bodyRotation); // inverse of rotation part odeUtils::getPositionAsDVec(visualsFrame, bodyPosition); dGeomSetBody(geom, getODEBody()); dGeomSetOffsetRotation(geom, bodyRotation); dGeomSetOffsetPosition(geom, bodyPosition[0], bodyPosition[1], bodyPosition[2]); // finally add all the visuals this->attatchODEVisuals(visualsFrame, getODEBody(), collisionSpaceID); // recurse into everything attached to this node for (KinematicNode *child : m_children) { dBodyID childBody = child->attachToODE(environment, visualSpaceID, collisionSpaceID, coordinateFrame); UNUSED(childBody); } return m_odeBody; }
static void command (int cmd) { int i,j,k; dReal sides[3]; dMass m; cmd = locase (cmd); if (cmd == 'b' || cmd == 's' || cmd == 'c' || cmd == 'x' || cmd == 'm' || cmd == 'y' ) { if (num < NUM) { i = num; num++; } else { i = nextobj; nextobj++; if (nextobj >= num) nextobj = 0; // destroy the body and geoms for slot i dBodyDestroy (obj[i].body); for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]); } memset (&obj[i],0,sizeof(obj[i])); } obj[i].body = dBodyCreate (world); for (k=0; k<3; k++) sides[k] = dRandReal()*0.5+0.1; dMatrix3 R; if (random_pos) { dBodySetPosition (obj[i].body, dRandReal()*2-1,dRandReal()*2-1,dRandReal()+3); dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); } else { dReal maxheight = 0; for (k=0; k<num; k++) { const dReal *pos = dBodyGetPosition (obj[k].body); if (pos[2] > maxheight) maxheight = pos[2]; } dBodySetPosition (obj[i].body, 0,0,maxheight+1); dRFromAxisAndAngle (R,0,0,1,dRandReal()*10.0-5.0); } dBodySetRotation (obj[i].body,R); dBodySetData (obj[i].body,(void*)(size_t)i); if (cmd == 'b') { dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]); obj[i].geom[0] = dCreateBox (space,sides[0],sides[1],sides[2]); } else if (cmd == 'c') { sides[0] *= 0.5; dMassSetCapsule (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCapsule (space,sides[0],sides[1]); } else if (cmd == 'y') { sides[1] *= 0.5; dMassSetCylinder (&m,DENSITY,3,sides[0],sides[1]); obj[i].geom[0] = dCreateCylinder (space,sides[0],sides[1]); } else if (cmd == 's') { sides[0] *= 0.5; dMassSetSphere (&m,DENSITY,sides[0]); obj[i].geom[0] = dCreateSphere (space,sides[0]); } else if (cmd == 'm') { dTriMeshDataID new_tmdata = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(new_tmdata, &Vertices[0], 3 * sizeof(float), VertexCount, (dTriIndex*)&Indices[0], IndexCount, 3 * sizeof(dTriIndex)); obj[i].geom[0] = dCreateTriMesh(space, new_tmdata, 0, 0, 0); // remember the mesh's dTriMeshDataID on its userdata for convenience. dGeomSetData(obj[i].geom[0], new_tmdata); dMassSetTrimesh( &m, DENSITY, obj[i].geom[0] ); printf("mass at %f %f %f\n", m.c[0], m.c[1], m.c[2]); dGeomSetPosition(obj[i].geom[0], -m.c[0], -m.c[1], -m.c[2]); dMassTranslate(&m, -m.c[0], -m.c[1], -m.c[2]); } else if (cmd == 'x') { dGeomID g2[GPB]; // encapsulated geometries dReal dpos[GPB][3]; // delta-positions for encapsulated geometries // start accumulating masses for the encapsulated geometries dMass m2; dMassSetZero (&m); // set random delta positions for (j=0; j<GPB; j++) { for (k=0; k<3; k++) dpos[j][k] = dRandReal()*0.3-0.15; } for (k=0; k<GPB; k++) { obj[i].geom[k] = dCreateGeomTransform (space); dGeomTransformSetCleanup (obj[i].geom[k],1); if (k==0) { dReal radius = dRandReal()*0.25+0.05; g2[k] = dCreateSphere (0,radius); dMassSetSphere (&m2,DENSITY,radius); } else if (k==1) { g2[k] = dCreateBox (0,sides[0],sides[1],sides[2]); dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]); } else { dReal radius = dRandReal()*0.1+0.05; dReal length = dRandReal()*1.0+0.1; g2[k] = dCreateCapsule (0,radius,length); dMassSetCapsule (&m2,DENSITY,3,radius,length); } dGeomTransformSetGeom (obj[i].geom[k],g2[k]); // set the transformation (adjust the mass too) dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]); dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]); dMatrix3 Rtx; dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0, dRandReal()*2.0-1.0,dRandReal()*10.0-5.0); dGeomSetRotation (g2[k],Rtx); dMassRotate (&m2,Rtx); // add to the total mass dMassAdd (&m,&m2); } // move all encapsulated objects so that the center of mass is (0,0,0) for (k=0; k<2; k++) { dGeomSetPosition (g2[k], dpos[k][0]-m.c[0], dpos[k][1]-m.c[1], dpos[k][2]-m.c[2]); } dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]); } for (k=0; k < GPB; k++) { if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body); } dBodySetMass (obj[i].body,&m); } if (cmd == ' ') { selected++; if (selected >= num) selected = 0; if (selected < 0) selected = 0; } else if (cmd == 'd' && selected >= 0 && selected < num) { dBodyDisable (obj[selected].body); } else if (cmd == 'e' && selected >= 0 && selected < num) { dBodyEnable (obj[selected].body); } else if (cmd == 'a') { show_aabb ^= 1; } else if (cmd == 't') { show_contacts ^= 1; } else if (cmd == 'r') { random_pos ^= 1; } }
void testMassFuncs(void) { PLmass m; plMassSet(&m, 5.0, 0.0, 0.0, 0.0, 10.0, 20.0, 30.0, 1.0, 2.0, 3.0); dMass dm; dMassSetParameters(&dm, 5.0, 0.0, 0.0, 0.0, 10.0, 20.0, 30.0, 1.0, 2.0, 3.0); printf("lm: %f : %f %f %f : %f %f %f\n", m.m, m.I[0][0], m.I[1][1], m.I[2][2], m.I[0][1], m.I[0][2], m.I[1][2]); printf("om: %f : %f %f %f : %f %f %f\n", dm.mass, dm.I[0*4+0], dm.I[1*4+1], dm.I[2*4+2], dm.I[0*4+1], dm.I[0*4+2], dm.I[1*4+2]); plMassTranslate(&m, 5.0, 10.0, 15.0); dMassTranslate(&dm, 5.0, 10.0, 15.0); printf("lmt: %f : %f %f %f : %f %f %f\n", m.m, m.I[0][0], m.I[1][1], m.I[2][2], m.I[0][1], m.I[0][2], m.I[1][2]); printf("omt: %f : %f %f %f : %f %f %f\n", dm.mass, dm.I[0*4+0], dm.I[1*4+1], dm.I[2*4+2], dm.I[0*4+1], dm.I[0*4+2], dm.I[1*4+2]); float3x3 mrot; mf3_rot(mrot, 1.0, 0.0, 0.0, 0.5); dReal rdr[16]; rdr[0] = mrot[0].x; rdr[1] = mrot[0].y; rdr[2] = mrot[0].z; rdr[3] = 0.0; rdr[4] = mrot[1].x; rdr[5] = mrot[1].y; rdr[6] = mrot[1].z; rdr[7] = 0.0; rdr[8] = mrot[2].x; rdr[9] = mrot[2].y; rdr[10] = mrot[2].z; rdr[11] = 0.0; rdr[12] = mrot[3].x; rdr[13] = mrot[3].y; rdr[14] = mrot[3].z; rdr[15] = 0.0; dMassRotate(&dm, rdr); plMassRotateM(&m, mrot); printf("lmr: %f : %f %f %f : %f %f %f : %f %f %f\n", m.m, m.I[0][0], m.I[1][1], m.I[2][2], m.I[0][1], m.I[0][2], m.I[1][2], m.cog.x, m.cog.y, m.cog.z); printf("omr: %f : %f %f %f : %f %f %f : %f %f %f\n", dm.mass, dm.I[0*4+0], dm.I[1*4+1], dm.I[2*4+2], dm.I[0*4+1], dm.I[0*4+2], dm.I[1*4+2], dm.c[0], dm.c[1], dm.c[2]); }