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]);
}
