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 TSRODERigidBody::AddCylinderGeometry( TSRPhysicsWorld* _pWorldInterface, const TSRMatrix4& _bodyToGeomTransform, float _fRadius,float _fLength, float _fDensity )
{
TSRODEPhysicsWorld* _pWorld = ( TSRODEPhysicsWorld* ) _pWorldInterface;
dMass totalMass;
dBodyGetMass( m_BodyID, &totalMass );
if ( m_GeomIDs.size() == 0 )
{
dMassSetZero( &totalMass );
}
dMatrix4 R;
dVector3 P;
Matrix4ToODE( _bodyToGeomTransform, R, P );
dGeomID geomTransform = dCreateGeomTransform( _pWorld->m_SpaceID );
dGeomID encapsulatedGeom = 0;
dMass currMass;
dMassSetZero( &currMass );
encapsulatedGeom = dCreateCylinder( 0, _fRadius, _fLength );
dMassSetCylinder( &currMass, _fDensity, 0, _fRadius, _fLength );
dMassRotate( &currMass, R );
//dMassTranslate(&currMass,P[0],P[1],P[2]);
dMassAdd( &totalMass, &currMass );
dGeomSetPosition( encapsulatedGeom, P[ 0 ], P[ 1 ], P[ 2 ] );
dGeomSetRotation( encapsulatedGeom, R );
dGeomTransformSetCleanup( geomTransform, 1 );
dGeomTransformSetGeom( geomTransform, encapsulatedGeom );
dGeomSetBody( geomTransform, m_BodyID );
m_GeomIDs.push_back( geomTransform );
dBodySetMass( m_BodyID, &totalMass );
}
void CBoxGeom::get_mass(dMass& m)
{
Fvector& hside=m_box.m_halfsize;
dMassSetBox(&m,1.f,hside.x*2.f,hside.y*2.f,hside.z*2.f);
dMatrix3 DMatx;
PHDynamicData::FMX33toDMX(m_box.m_rotate,DMatx);
dMassRotate(&m,DMatx);
}
// 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 CCylinderGeom::get_mass(dMass& m)
{
dMassSetCylinder(&m,1.f,2,m_cylinder.m_radius,m_cylinder.m_height);
dMatrix3 DMatx;
Fmatrix33 m33;
m33.j.set(m_cylinder.m_direction);
Fvector::generate_orthonormal_basis(m33.j,m33.k,m33.i);
PHDynamicData::FMX33toDMX(m33,DMatx);
dMassRotate(&m,DMatx);
}
void HarrierSim::makeHarrier()
{
dMass mass;
itsHarrierBody = dBodyCreate(world);
//pos[0] = 0; pos[1] = 1.0*5; pos[2] = 1.80;
dBodySetPosition(itsHarrierBody,0,0.2*5,4.94);
dMatrix3 R;
dRFromAxisAndAngle (R,1,0,0,0);
dBodySetRotation(itsHarrierBody, R);
dMassSetZero(&mass);
dMassSetBoxTotal(&mass, itsHarrierWeight,itsHarrierWidth, itsHarrierLength, 0.5);
//dMassSetCappedCylinderTotal(&mass,itsHarrierWeight,3,itsHarrierWidth,itsHarrierLength/2);
dMassRotate(&mass, R);
dBodySetMass(itsHarrierBody,&mass);
itsHarrierGeom = dCreateBox(space, itsHarrierWidth, itsHarrierLength, 0.5);
dGeomSetRotation(itsHarrierGeom, R);
dGeomSetBody(itsHarrierGeom, itsHarrierBody);
}
void PhysicsBody::rotateMass( const Matrix& R)
{
dMatrix3 rotation;
Vec4f v1 = R[0];
Vec4f v2 = R[1];
Vec4f v3 = R[2];
rotation[0] = v1.x();
rotation[1] = v1.y();
rotation[2] = v1.z();
rotation[3] = 0;
rotation[4] = v2.x();
rotation[5] = v2.y();
rotation[6] = v2.z();
rotation[7] = 0;
rotation[8] = v3.x();
rotation[9] = v3.y();
rotation[10] = v3.z();
rotation[11] = 0;
dMass mass;
getMassStruct(mass);
dMassRotate(&mass, rotation);
setMassStruct(mass);
}
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;
}
}
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;
}
}
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;
}
}
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;
}
}
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]);
}
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");
}
void createTest()
{
int i,j;
if (world) dWorldDestroy (world);
world = dWorldCreate();
// create random bodies
for (i=0; i<NUM; i++) {
// create bodies at random position and orientation
body[i] = dBodyCreate (world);
// dBodySetPosition (body[i],dRandReal()*2-1,dRandReal()*2-1,
// dRandReal()*2+RADIUS);
dBodySetPosition (body[i],dRandReal()*SIDE-1,dRandReal()*SIDE-1,
dRandReal()*SIDE+RADIUS);
dReal q[4];
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dBodySetQuaternion (body[i],q);
// set random velocity
dBodySetLinearVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
dBodySetAngularVel (body[i], dRandReal()*2-1,dRandReal()*2-1,
dRandReal()*2-1);
// set random mass (random diagonal mass rotated by a random amount)
dMass m;
dMatrix3 R;
dMassSetBox (&m,1,dRandReal()+0.1,dRandReal()+0.1,dRandReal()+0.1);
dMassAdjust (&m,dRandReal()+1);
for (j=0; j<4; j++) q[j] = dRandReal()*2-1;
dQtoR (q,R);
dMassRotate (&m,R);
dBodySetMass (body[i],&m);
}
// create ball-n-socket joints at random positions, linking random bodies
// (but make sure not to link the same pair of bodies twice)
for (i=0; i<NUM*NUM; i++) linked[i] = 0;
for (i=0; i<NUMJ; i++) {
int b1,b2;
do {
b1 = dRandInt (NUM);
b2 = dRandInt (NUM);
} while (linked[b1*NUM + b2] || b1==b2);
linked[b1*NUM + b2] = 1;
linked[b2*NUM + b1] = 1;
joint[i] = dJointCreateBall (world,0);
dJointAttach (joint[i],body[b1],body[b2]);
dJointSetBallAnchor (joint[i],dRandReal()*2-1,
dRandReal()*2-1,dRandReal()*2+RADIUS);
}
for (i=0; i<NUM; i++) {
// move bodies a bit to get some joint error
const dReal *pos = dBodyGetPosition (body[i]);
dBodySetPosition (body[i],pos[0]+dRandReal()*0.2-0.1,
pos[1]+dRandReal()*0.2-0.1,pos[2]+dRandReal()*0.2-0.1);
}
}
