void RigidBody::setMeshTransfomation()
{
if(m_type !=TRIANGLEMESH)
{
std::cerr<<"trying to set transform on non mesh\n";
return;
}
const dReal *rot=dBodyGetRotation(m_id);
const dReal *pos=dBodyGetPosition(m_id);
dReal tx[16];
tx[0]=rot[0];
tx[1]=rot[1];
tx[2]=rot[2];
tx[3]=pos[0];
tx[4]=rot[4];
tx[5]=rot[5];
tx[6]=rot[6];
tx[7]=pos[1];
tx[8]=rot[8];
tx[9]=rot[9];
tx[10]=rot[10];
tx[11]=pos[2];
tx[12]=rot[12];
tx[13]=rot[13];
tx[14]=rot[14];
tx[15]=1.0;
dGeomTriMeshSetLastTransform( m_geom.getID() ,tx );
dGeomTriMeshClearTCCache (m_geom.getID());
}
void Solid::prepare()
{
memcpy(m_last_transform,m_current_transform,16) ;
dGeomTriMeshSetLastTransform(m_geometry1,m_last_transform) ;
const dReal* Pos = dGeomGetPosition(m_geometry1);
const dReal* Rot = dGeomGetRotation(m_geometry1);
m_current_transform[ 0 ] = Rot[ 0 ]; m_current_transform[ 1 ] = Rot[ 1 ]; m_current_transform[ 2 ] = Rot[ 2 ]; m_current_transform[ 3 ] = 0;
m_current_transform[ 4 ] = Rot[ 4 ]; m_current_transform[ 5 ] = Rot[ 5 ]; m_current_transform[ 6 ] = Rot[ 6 ]; m_current_transform[ 7 ] = 0;
m_current_transform[ 8 ] = Rot[ 8 ]; m_current_transform[ 9 ] = Rot[ 9 ]; m_current_transform[10 ] = Rot[10 ]; m_current_transform[11 ] = 0;
m_current_transform[12 ] = Pos[ 0 ]; m_current_transform[13 ] = Pos[ 1 ]; m_current_transform[14 ] = Pos[ 2 ]; m_current_transform[15 ] = 1;
}
void trimesh::update()
{
double qx, qy, qz, qw, x, y, z;
get_orientation(qx, qy, qz, qw);
get_position(x, y, z);
quaterniond quat(qx, qy, qz, qw);
matrix_3dd matrix;
quat.create_matrix(matrix);
matrix.translate(x, y, z);
dReal* matrix_data = (dReal*)matrix.raw_matrix();
dGeomTriMeshSetLastTransform(geom_id, matrix_data);
}
// set previous transformation matrix for trimesh
void setCurrentTransform(dGeomID geom)
{
const dReal* Pos = dGeomGetPosition(geom);
const dReal* Rot = dGeomGetRotation(geom);
const dReal Transform[16] =
{
Rot[0], Rot[4], Rot[8], 0,
Rot[1], Rot[5], Rot[9], 0,
Rot[2], Rot[6], Rot[10], 0,
Pos[0], Pos[1], Pos[2], 1
};
dGeomTriMeshSetLastTransform( geom, *(dMatrix4*)(&Transform) );
}
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
#if 1
// What is this for??? - Bram
if (!pause)
{
for (int i=0; i<num; i++)
for (int j=0; j < GPB; j++)
if (obj[i].geom[j])
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass)
setCurrentTransform(obj[i].geom[j]);
setCurrentTransform(TriMesh1);
setCurrentTransform(TriMesh2);
}
#endif
//if (!pause) dWorldStep (world,0.05);
if (!pause) dWorldQuickStep (world,0.05);
for (int j = 0; j < dSpaceGetNumGeoms(space); j++){
dSpaceGetGeom(space, j);
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=0; i<num; i++) {
for (int j=0; j < GPB; j++) {
if (obj[i].geom[j]) {
if (i==selected) {
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
}
else {
dsSetColor (1,1,0);
}
if (dGeomGetClass(obj[i].geom[j]) == dTriMeshClass) {
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
for (int ii = 0; ii < IndexCount / 3; ii++) {
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
// tell the tri-tri collider the current transform of the trimesh --
// this is fairly important for good results.
// Fill in the (4x4) matrix.
dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
// Flip to other matrix.
obj[i].last_matrix_index = !obj[i].last_matrix_index;
dGeomTriMeshSetLastTransform( obj[i].geom[j],
*(dMatrix4*)( obj[i].matrix_dblbuff + obj[i].last_matrix_index * 16 ) );
} else {
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
}
dTriIndex* Indices = (dTriIndex*)::Indices;
{const dReal* Pos = dGeomGetPosition(TriMesh1);
const dReal* Rot = dGeomGetRotation(TriMesh1);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 0);
}}}
{const dReal* Pos = dGeomGetPosition(TriMesh2);
const dReal* Rot = dGeomGetRotation(TriMesh2);
{for (int i = 0; i < IndexCount / 3; i++){
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[i * 3 + 0] * 3 + 0],
Vertices[Indices[i * 3 + 0] * 3 + 1],
Vertices[Indices[i * 3 + 0] * 3 + 2],
Vertices[Indices[i * 3 + 1] * 3 + 0],
Vertices[Indices[i * 3 + 1] * 3 + 1],
Vertices[Indices[i * 3 + 1] * 3 + 2],
Vertices[Indices[i * 3 + 2] * 3 + 0],
Vertices[Indices[i * 3 + 2] * 3 + 1],
Vertices[Indices[i * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}}}
}
//===========================================================================
void cODEGenericBody::updateBodyPosition(void)
{
const double *odePosition;
const double *odeRotation;
// Retrieve position and orientation from ODE body.
if (m_ode_body != NULL)
{
odePosition = dBodyGetPosition(m_ode_body);
odeRotation = dBodyGetRotation(m_ode_body);
}
else
{
return;
}
// set new position
m_localPos.set(odePosition[0],odePosition[1],odePosition[2]);
// set new orientation
m_localRot.set(odeRotation[0],odeRotation[1],odeRotation[2],
odeRotation[4],odeRotation[5],odeRotation[6],
odeRotation[8],odeRotation[9],odeRotation[10]);
// store previous position if object is a mesh
if (m_ode_triMeshDataID != NULL)
{
m_prevTransform[0] = odeRotation[0];
m_prevTransform[1] = odeRotation[4];
m_prevTransform[2] = odeRotation[8];
m_prevTransform[3] = 0.0;
m_prevTransform[4] = odeRotation[1];
m_prevTransform[5] = odeRotation[5];
m_prevTransform[6] = odeRotation[9];
m_prevTransform[7] = 0.0;
m_prevTransform[8] = odeRotation[2];
m_prevTransform[9] = odeRotation[6];
m_prevTransform[10] = odeRotation[10];
m_prevTransform[11] = 0.0;
m_prevTransform[12] = odePosition[0];
m_prevTransform[13] = odePosition[1];
m_prevTransform[14] = odePosition[2];
m_prevTransform[15] = 1.0;
dGeomTriMeshSetLastTransform(m_ode_geom, m_prevTransform);
}
// Normalize frame
// This can be useful is ODE is running in SINGLE precision mode
// where precision is a problem
/*
cVector3d c0(odeRotation[0], odeRotation[4], odeRotation[8]);
cVector3d c1(odeRotation[1], odeRotation[5], odeRotation[9]);
cVector3d c2(odeRotation[2], odeRotation[6], odeRotation[10]);
c0.crossr(c1, c2);
c2.crossr(c0, c1);
c0.normalize();
c1.normalize();
c2.normalize();
// set new orientation
m_localRot.setCol(c0, c1, c2);
*/
}
static void simLoop (int pause)
{
int i,j;
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
//if (!pause) dWorldStep (world,0.05);
//if (!pause) dWorldQuickStep (world,0.05);
if (!pause) dWorldStepFast1 (world,0.05, 5);
if (write_world) {
FILE *f = fopen ("state.dif","wt");
if (f) {
dWorldExportDIF (world,f,"X");
fclose (f);
}
write_world = 0;
}
// remove all contact joints
dJointGroupEmpty (contactgroup);
const dReal* pReal = dGeomGetPosition( gheight );
const dReal* RReal = dGeomGetRotation( gheight );
//
// Draw Heightfield
//
// Set ox and oz to zero for DHEIGHTFIELD_CORNER_ORIGIN mode.
int ox = (int) ( -HFIELD_WIDTH/2 );
int oz = (int) ( -HFIELD_DEPTH/2 );
// for ( int tx = -1; tx < 2; ++tx )
// for ( int tz = -1; tz < 2; ++tz )
{
dsSetColorAlpha (0.5,1,0.5,0.5);
dsSetTexture( DS_WOOD );
for ( int i = 0; i < HFIELD_WSTEP - 1; ++i )
for ( int j = 0; j < HFIELD_DSTEP - 1; ++j )
{
dReal a[3], b[3], c[3], d[3];
a[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
a[ 1 ] = heightfield_callback( NULL, i, j );
a[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
b[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
b[ 1 ] = heightfield_callback( NULL, i + 1, j );
b[ 2 ] = oz + ( j ) * HFIELD_DSAMP;
c[ 0 ] = ox + ( i ) * HFIELD_WSAMP;
c[ 1 ] = heightfield_callback( NULL, i, j + 1 );
c[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
d[ 0 ] = ox + ( i + 1 ) * HFIELD_WSAMP;
d[ 1 ] = heightfield_callback( NULL, i + 1, j + 1 );
d[ 2 ] = oz + ( j + 1 ) * HFIELD_DSAMP;
dsDrawTriangle( pReal, RReal, a, c, b, 1 );
dsDrawTriangle( pReal, RReal, b, c, d, 1 );
}
}
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (i=0; i<num; i++)
{
for (j=0; j < GPB; j++)
{
if (i==selected)
{
dsSetColor (0,0.7,1);
}
else if (! dBodyIsEnabled (obj[i].body))
{
dsSetColor (1,0.8,0);
}
else
{
dsSetColor (1,1,0);
}
if ( obj[i].geom[j] && dGeomGetClass(obj[i].geom[j]) == dTriMeshClass )
{
dTriIndex* Indices = (dTriIndex*)::Indices;
// assume all trimeshes are drawn as bunnies
const dReal* Pos = dGeomGetPosition(obj[i].geom[j]);
const dReal* Rot = dGeomGetRotation(obj[i].geom[j]);
for (int ii = 0; ii < IndexCount / 3; ii++)
{
const dReal v[9] = { // explicit conversion from float to dReal
Vertices[Indices[ii * 3 + 0] * 3 + 0],
Vertices[Indices[ii * 3 + 0] * 3 + 1],
Vertices[Indices[ii * 3 + 0] * 3 + 2],
Vertices[Indices[ii * 3 + 1] * 3 + 0],
Vertices[Indices[ii * 3 + 1] * 3 + 1],
Vertices[Indices[ii * 3 + 1] * 3 + 2],
Vertices[Indices[ii * 3 + 2] * 3 + 0],
Vertices[Indices[ii * 3 + 2] * 3 + 1],
Vertices[Indices[ii * 3 + 2] * 3 + 2]
};
dsDrawTriangle(Pos, Rot, &v[0], &v[3], &v[6], 1);
}
// tell the tri-tri collider the current transform of the trimesh --
// this is fairly important for good results.
// Fill in the (4x4) matrix.
dReal* p_matrix = obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 );
p_matrix[ 0 ] = Rot[ 0 ]; p_matrix[ 1 ] = Rot[ 1 ]; p_matrix[ 2 ] = Rot[ 2 ]; p_matrix[ 3 ] = 0;
p_matrix[ 4 ] = Rot[ 4 ]; p_matrix[ 5 ] = Rot[ 5 ]; p_matrix[ 6 ] = Rot[ 6 ]; p_matrix[ 7 ] = 0;
p_matrix[ 8 ] = Rot[ 8 ]; p_matrix[ 9 ] = Rot[ 9 ]; p_matrix[10 ] = Rot[10 ]; p_matrix[11 ] = 0;
p_matrix[12 ] = Pos[ 0 ]; p_matrix[13 ] = Pos[ 1 ]; p_matrix[14 ] = Pos[ 2 ]; p_matrix[15 ] = 1;
// Flip to other matrix.
obj[i].last_matrix_index = !obj[i].last_matrix_index;
// Apply the 'other' matrix which is the oldest.
dGeomTriMeshSetLastTransform( obj[i].geom[j],
*(dMatrix4*)( obj[i].matrix_dblbuff + ( obj[i].last_matrix_index * 16 ) ) );
}
else
{
drawGeom (obj[i].geom[j],0,0,show_aabb);
}
}
}
if ( show_aabb )
{
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (gheight,aabb);
dVector3 bbpos;
for (i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (i=0; i<3; i++) bbsides[i] = aabb[i*2+1] - aabb[i*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
