void dJointSetUniversalParam( dJointID j, int parameter, dReal value )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if (( parameter & 0xff00 ) == 0x100 )
{
joint->limot2.set( parameter & 0xff, value );
}
else
{
joint->limot1.set( parameter, value );
}
}
dReal dJointGetUniversalParam( dJointID j, int parameter )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if (( parameter & 0xff00 ) == 0x100 )
{
return joint->limot2.get( parameter & 0xff );
}
else
{
return joint->limot1.get( parameter );
}
}
void dGeomRaySetParams (dxGeom *g, int FirstContact, int BackfaceCull)
{
dUASSERT (g && g->type == dRayClass,"argument not a ray");
if (FirstContact){
g->gflags |= RAY_FIRSTCONTACT;
}
else g->gflags &= ~RAY_FIRSTCONTACT;
if (BackfaceCull){
g->gflags |= RAY_BACKFACECULL;
}
else g->gflags &= ~RAY_BACKFACECULL;
}
dReal dJointGetGearboxParam( dJointID j, int parameter )
{
dxJointGearbox* joint = static_cast<dxJointGearbox*>(j);
dUASSERT( joint, "bad joint argument" );
switch ( parameter ) {
case dParamCFM:
return joint->cfm;
case dParamERP:
return joint->erp;
default:
return 0;
}
}
void dJointGetUniversalAngles( dJointID j, dReal *angle1, dReal *angle2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
joint->getAngles( angle2, angle1 );
*angle2 = -(*angle2);
return;
}
else
return joint->getAngles( angle1, angle2 );
}
void dJointSetGearboxReferenceBody2( dJointID j, dBodyID b )
{
dxJointGearbox* joint = dynamic_cast<dxJointGearbox*>(j);
dUASSERT( joint, "bad joint argument" );
dUASSERT( b, "bad body argument" );
joint->refBody2 = b;
if ( joint->node[1].body )
{
if ( b )
dQMultiply1( joint->qrel2, joint->refBody2->q, joint->node[1].body->q );
else
{
// set qrel1 to the transpose of the first body q
joint->qrel2[0] = joint->node[1].body->q[0];
joint->qrel2[1] = joint->node[1].body->q[1];
joint->qrel2[2] = joint->node[1].body->q[2];
joint->qrel2[3] = joint->node[1].body->q[3];
}
}
else
{
if ( b )
{
// set qrel2 to the transpose of the second body q
joint->qrel2[0] = joint->refBody2->q[0];
joint->qrel2[1] = - joint->refBody2->q[1];
joint->qrel2[2] = - joint->refBody2->q[2];
joint->qrel2[3] = - joint->refBody2->q[3];
}
else
{
// both refBody2 and node[1].body are null, nothing happens
}
}
}
void dGeomTriMeshDataSet(dTriMeshDataID g, int data_id, void* in_data)
{
dUASSERT(g, "argument not trimesh data");
double *elem;
switch (data_id) {
case TRIMESH_FACE_NORMALS:
g->Normals = (dReal *) in_data;
break;
case TRIMESH_LAST_TRANSFORMATION:
elem = (double *) in_data;
for (int i=0; i<16; i++)
g->last_trans[i] = (dReal) elem[i];
break;
default:
dUASSERT(data_id, "invalid data type");
break;
}
return;
}
int dCreateGeomClass (const dGeomClass *c)
{
dUASSERT(c && c->bytes >= 0 && c->collider && c->aabb,"bad geom class");
if (num_user_classes >= dMaxUserClasses) {
dDebug (0,"too many user classes, you must increase the limit and "
"recompile ODE");
}
user_classes[num_user_classes] = *c;
int class_number = num_user_classes + dFirstUserClass;
setAllColliders (class_number,&dCollideUserGeomWithGeom);
num_user_classes++;
return class_number;
}
dReal dJointGetScrewParam( dJointID j, int parameter )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
switch (parameter)
{
case dParamERP:
return joint->erp;
case dParamCFM:
return joint->cfm;
default:
return joint->limot.get( parameter );
}
}
void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->body) {
const dReal * body_quat = dBodyGetQuaternion (g->body);
quat[0] = body_quat[0];
quat[1] = body_quat[1];
quat[2] = body_quat[2];
quat[3] = body_quat[3];
}
else {
dRtoQ (g->R, quat);
}
}
void dJointSetHinge2Axis1( dJointID j, dReal x, dReal y, dReal z )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body )
{
setAxes(joint, x, y, z, joint->axis1, NULL);
// compute the sin and cos of the angle between axis 1 and axis 2
dVector3 ax1, ax2, ax;
joint->getAxisInfo( ax1, ax2, ax, joint->s0, joint->c0 );
}
joint->makeV1andV2();
}
void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
const dMatrix3 &src = g->buildUpdatedRotation();
R[0] = src[dM3E_XX];
R[1] = src[dM3E_XY];
R[2] = src[dM3E_XZ];
R[4] = src[dM3E_YX];
R[5] = src[dM3E_YY];
R[6] = src[dM3E_YZ];
R[8] = src[dM3E_ZX];
R[9] = src[dM3E_ZY];
R[10] = src[dM3E_ZZ];
}
dReal dJointGetHinge2Param( dJointID j, int parameter )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if (( parameter & 0xff00 ) == 0x100 )
{
return joint->limot2.get( parameter & 0xff );
}
else
{
if ( parameter == dParamSuspensionERP ) return joint->susp_erp;
else if ( parameter == dParamSuspensionCFM ) return joint->susp_cfm;
else return joint->limot1.get( parameter );
}
}
void dGeomGetQuaternion (dxGeom *g, dQuaternion quat)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->body && !g->offset_posr) {
const dReal * body_quat = dBodyGetQuaternion (g->body);
quat[0] = body_quat[0];
quat[1] = body_quat[1];
quat[2] = body_quat[2];
quat[3] = body_quat[3];
}
else {
g->recomputePosr();
dRtoQ (g->final_posr->R, quat);
}
}
void dGeomSetPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
CHECK_NOT_LOCKED (g->parent_space);
if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetPosition (g->body,x,y,z);
}
else {
g->pos[0] = x;
g->pos[1] = y;
g->pos[2] = z;
dGeomMoved (g);
}
}
void dGeomCopyRotation(dxGeom *g, dMatrix3 R)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
g->recomputePosr();
const dReal* src = g->final_posr->R;
R[0] = src[0];
R[1] = src[1];
R[2] = src[2];
R[4] = src[4];
R[5] = src[5];
R[6] = src[6];
R[8] = src[8];
R[9] = src[9];
R[10] = src[10];
}
dReal dJointGetHinge2Angle2Rate( dJointID j )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body && joint->node[1].body )
{
dVector3 axis;
dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
if ( joint->node[1].body )
rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
return rate;
}
else return 0;
}
void dJointSetHinge2Param( dJointID j, int parameter, dReal value )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if (( parameter & 0xff00 ) == 0x100 )
{
joint->limot2.set( parameter & 0xff, value );
}
else
{
if ( parameter == dParamSuspensionERP ) joint->susp_erp = value;
else if ( parameter == dParamSuspensionCFM ) joint->susp_cfm = value;
else joint->limot1.set( parameter, value );
}
}
void dGeomTriMeshEnableTC(dGeomID g, int geomClass, int enable)
{
dUASSERT(g && g->type == dTriMeshClass, "argument not a trimesh");
switch (geomClass)
{
case dSphereClass:
((dxTriMesh*)g)->doSphereTC = (1 == enable);
break;
case dBoxClass:
((dxTriMesh*)g)->doBoxTC = (1 == enable);
break;
case dCCylinderClass:
((dxTriMesh*)g)->doCCylinderTC = (1 == enable);
break;
}
}
void dGeomClearOffset(dxGeom *g)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
if (g->offset_posr)
{
dIASSERT(g->body);
// no longer need an offset posr
dFreePosr(g->offset_posr);
g->offset_posr = 0;
// the geom will now share the position of the body
dFreePosr(g->final_posr);
g->final_posr = &g->body->posr;
// geom has moved
g->gflags &= ~GEOM_POSR_BAD;
dGeomMoved (g);
}
}
void dxQuadTreeSpace::add(dxGeom* g){
CHECK_NOT_LOCKED (this);
dAASSERT(g);
dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");
g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
DirtyList.push(g);
// add
g->parent_space = this;
Blocks[0].GetBlock(g->aabb)->AddObject(g); // Add to best block
count++;
// enumerator has been invalidated
current_geom = 0;
dGeomMoved(this);
}
void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dVector3 axis1, axis2;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body && joint->node[1].body )
{
dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
}
}
void dxQuadTreeSpace::remove(dxGeom* g){
CHECK_NOT_LOCKED(this);
dAASSERT(g);
dUASSERT(g->parent_space == this,"object is not in this space");
// remove
((Block*)g->tome_ex)->DelObject(g);
for (int i = 0; i < DirtyList.size(); i++){
if (DirtyList[i] == g){
DirtyList.remove(i);
// (mg) there can be multiple instances of a dirty object on stack be sure to remove ALL and not just first, for this we decrement i
--i;
}
}
dxSpace::remove(g);
}
void dxSAPSpace::add( dxGeom* g )
{
CHECK_NOT_LOCKED (this);
dAASSERT(g);
dUASSERT(g->parent_space == 0 && g->next == 0, "geom is already in a space");
g->gflags |= GEOM_DIRTY | GEOM_AABB_BAD;
// add to dirty list
GEOM_SET_DIRTY_IDX( g, DirtyList.size() );
GEOM_SET_GEOM_IDX( g, GEOM_INVALID_IDX );
DirtyList.push( g );
g->parent_space = this;
this->count++;
dGeomMoved(this);
}
void dJointSetScrewAxisOffset( dJointID j, dReal x, dReal y, dReal z, dReal dangle )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
setAxes( joint, x, y, z, joint->axis1, joint->axis2 );
joint->computeInitialRelativeRotation();
if ( joint->flags & dJOINT_REVERSE ) dangle = -dangle;
dQuaternion qAngle, qOffset;
dQFromAxisAndAngle(qAngle, x, y, z, dangle);
dQMultiply3(qOffset, qAngle, joint->qrel);
joint->qrel[0] = qOffset[0];
joint->qrel[1] = qOffset[1];
joint->qrel[2] = qOffset[2];
joint->qrel[3] = qOffset[3];
}
void dMassSetCylinderTotal (dMass *m, dReal total_mass, int direction,
dReal radius, dReal length)
{
dReal r2,I;
dAASSERT (m);
dUASSERT (direction >= 1 && direction <= 3,"bad direction number");
dMassSetZero (m);
r2 = radius*radius;
m->mass = total_mass;
I = total_mass*(REAL(0.25)*r2 + (REAL(1.0)/REAL(12.0))*length*length);
m->_I(0,0) = I;
m->_I(1,1) = I;
m->_I(2,2) = I;
m->_I(direction-1,direction-1) = total_mass*REAL(0.5)*r2;
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dJointSetPUParam( dJointID j, int parameter, dReal value )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
switch ( parameter & 0xff00 )
{
case dParamGroup1:
joint->limot1.set( parameter, value );
break;
case dParamGroup2:
joint->limot2.set( parameter & 0xff, value );
break;
case dParamGroup3:
joint->limotP.set( parameter & 0xff, value );
break;
}
}
void dJointSetSliderAxisDelta ( dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz )
{
dxJointSlider* joint = ( dxJointSlider* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Slider );
setAxes ( joint, x, y, z, joint->axis1, 0 );
joint->computeOffset();
// compute initial relative rotation body1 -> body2, or env -> body1
// also compute center of body1 w.r.t body 2
if ( !(joint->node[1].body) )
{
joint->offset[0] += dx;
joint->offset[1] += dy;
joint->offset[2] += dz;
}
joint->computeInitialRelativeRotation();
}
void dJointSetScrewParam( dJointID j, int parameter, dReal value )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
switch (parameter)
{
case dParamERP:
joint->erp = value;
break;
case dParamCFM:
joint->cfm = value;
// dParamCFM label is also used for normal_cfm
joint->limot.set( parameter, value );
break;
default:
joint->limot.set( parameter, value );
break;
}
}
void dJointAddScrewForce ( dJointID j, dReal force )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dVector3 axis;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
if ( joint->flags & dJOINT_REVERSE )
force -= force;
getAxis ( joint, axis, joint->axis1 );
axis[0] *= force;
axis[1] *= force;
axis[2] *= force;
if ( joint->node[0].body != 0 )
dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
if ( joint->node[1].body != 0 )
dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
{
// linear torque decoupling:
// we have to compensate the torque, that this screw force may generate
// if body centers are not aligned along the screw axis
dVector3 ltd; // Linear Torque Decoupling vector (a torque)
dVector3 cgdiff;
cgdiff[0] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[0] -
joint->node[0].body->posr.pos[0] );
cgdiff[1] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[1] -
joint->node[0].body->posr.pos[1] );
cgdiff[2] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[2] -
joint->node[0].body->posr.pos[2] );
dCalcVectorCross3( ltd, cgdiff, axis );
dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
}
}