void
dxJointHinge2::makeV1andV2()
{
if ( node[0].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, v;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if ((_dequal(ax1[0], 0.0) && _dequal(ax1[1], 0.0) && _dequal(ax1[2], 0.0)) ||
(_dequal(ax2[0], 0.0) && _dequal(ax2[1], 0.0) && _dequal(ax2[2], 0.0)) ||
(_dequal(ax1[0], ax2[0]) && _dequal(ax1[1], ax2[1]) && _dequal(ax1[2], ax2[2])))
return;
// modify axis 2 so it's perpendicular to axis 1
dReal k = dCalcVectorDot3( ax1, ax2 );
for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
dNormalize3( ax2 );
// make v1 = modified axis2, v2 = axis1 x (modified axis2)
dCalcVectorCross3( v, ax1, ax2 );
dMultiply1_331( v1, node[0].body->posr.R, ax2 );
dMultiply1_331( v2, node[0].body->posr.R, v );
}
}
void
dxJointAMotor::setEulerReferenceVectors()
{
if ( node[0].body && node[1].body )
{
dVector3 r; // axis[2] and axis[0] in global coordinates
dMultiply0_331( r, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
dMultiply1_331( reference2, node[1].body->posr.R, r );
}
else // jds
{
// else if (j->node[0].body) {
// dMultiply1_331 (j->reference1,j->node[0].body->posr.R,j->axis[2]);
// dMultiply0_331 (j->reference2,j->node[0].body->posr.R,j->axis[0]);
// We want to handle angular motors attached to passive geoms
dVector3 r; // axis[2] and axis[0] in global coordinates
r[0] = axis[2][0];
r[1] = axis[2][1];
r[2] = axis[2][2];
r[3] = axis[2][3];
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
reference2[0] += r[0];
reference2[1] += r[1];
reference2[2] += r[2];
reference2[3] += r[3];
}
}
void
dxJointHinge2::makeW1andW2()
{
if ( node[1].body )
{
// get axis 1 and 2 in global coords
dVector3 ax1, ax2, w;
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
// don't do anything if the axis1 or axis2 vectors are zero or the same
if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;
// modify axis 1 so it's perpendicular to axis 2
dReal k = dCalcVectorDot3( ax2, ax1 );
for ( int i = 0; i < 3; i++ ) ax1[i] -= k * ax2[i];
dNormalize3( ax1 );
// make w1 = modified axis1, w2 = axis2 x (modified axis1)
dCalcVectorCross3( w, ax2, ax1 );
dMultiply1_331( w1, node[1].body->posr.R, ax1 );
dMultiply1_331( w2, node[1].body->posr.R, w );
}
}
void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
if ( joint->rel[anum] > 0 )
{
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// If we're in Euler mode, joint->axis[1] doesn't
// have anything sensible in it. So don't just return
// that, find the actual effective axis.
// Likewise, the actual axis of rotation for the
// the other axes is different from what's stored.
if ( joint->mode == dAMotorEuler ) {
dVector3 axes[3];
joint->computeGlobalAxes(axes);
if (anum == 1) {
result[0]=axes[1][0];
result[1]=axes[1][1];
result[2]=axes[1][2];
} else if (anum == 0) {
// This won't be unit length in general,
// but it's what's used in getInfo2
// This may be why things freak out as
// the body-relative axes get close to each other.
dCalcVectorCross3( result, axes[1], axes[2] );
} else if (anum == 2) {
// Same problem as above.
dCalcVectorCross3( result, axes[0], axes[1] );
}
} else if ( joint->rel[anum] > 0 ) {
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
////////////////////////////////////////////////////////////////////////////////
/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
/// relative to body 1 and 2 initially) and then computes the constrained
/// rotational axis as the cross product of ax1 and ax2.
/// the sin and cos of the angle between axis 1 and 2 is computed, this comes
/// from dot and cross product rules.
///
/// @param ax1 Will contain the joint axis1 in world frame
/// @param ax2 Will contain the joint axis2 in world frame
/// @param axis Will contain the cross product of ax1 x ax2
/// @param sin_angle
/// @param cos_angle
////////////////////////////////////////////////////////////////////////////////
void
dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
dReal &sin_angle, dReal &cos_angle) const
{
dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
dCalcVectorCross3(axCross,ax1,ax2);
sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
cos_angle = dCalcVectorDot3 (ax1,ax2);
}
// compute the 3 axes in global coordinates
void
dxJointAMotor::computeGlobalAxes( dVector3 ax[3] )
{
if ( mode == dAMotorEuler )
{
// special handling for euler mode
dMultiply0_331( ax[0], node[0].body->posr.R, axis[0] );
if ( node[1].body )
{
dMultiply0_331( ax[2], node[1].body->posr.R, axis[2] );
}
else
{
ax[2][0] = axis[2][0];
ax[2][1] = axis[2][1];
ax[2][2] = axis[2][2];
}
dCalcVectorCross3( ax[1], ax[2], ax[0] );
dNormalize3( ax[1] );
}
else
{
for ( int i = 0; i < num; i++ )
{
if ( rel[i] == 1 )
{
// relative to b1
dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
}
else if ( rel[i] == 2 )
{
// relative to b2
if ( node[1].body ) // jds: don't assert, just ignore
{
dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
}
else
{
// global - just copy it
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
else
{
// global - just copy it
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
}
}
dReal dJointGetPRPosition( dJointID j )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PR );
dVector3 q;
// get the offset in global coordinates
dMultiply0_331( q, joint->node[0].body->posr.R, joint->offset );
if ( joint->node[1].body )
{
dVector3 anchor2;
// get the anchor2 in global coordinates
dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 );
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->node[1].body->posr.pos[0] + anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->node[1].body->posr.pos[1] + anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->node[1].body->posr.pos[2] + anchor2[2] ) );
}
else
{
//N.B. When there is no body 2 the joint->anchor2 is already in
// global coordinates
q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) -
( joint->anchor2[0] ) );
q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) -
( joint->anchor2[1] ) );
q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) -
( joint->anchor2[2] ) );
if ( joint->flags & dJOINT_REVERSE )
{
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
}
}
dVector3 axP;
// get prismatic axis in global coordinates
dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 );
return dCalcVectorDot3( axP, q );
}
void getAxis( dxJoint *j, dVector3 result, dVector3 axis1 )
{
if ( j->node[0].body )
{
dMultiply0_331( result, j->node[0].body->posr.R, axis1 );
}
}
void dMassRotate (dMass *m, const dMatrix3 R)
{
// if the body is rotated by `R' relative to its point of reference,
// the new inertia about the point of reference is:
//
// R * I * R'
//
// where I is the old inertia.
dMatrix3 t1;
dReal t2[3];
dAASSERT (m);
// rotate inertia matrix
dMultiply2_333 (t1,m->I,R);
dMultiply0_333 (m->I,R,t1);
// ensure perfect symmetry
m->_I(1,0) = m->_I(0,1);
m->_I(2,0) = m->_I(0,2);
m->_I(2,1) = m->_I(1,2);
// rotate center of mass
dMultiply0_331 (t2,R,m->c);
m->c[0] = t2[0];
m->c[1] = t2[1];
m->c[2] = t2[2];
# ifndef dNODEBUG
dMassCheck (m);
# endif
}
void dxTriMesh::computeAABB() {
const dxTriMeshData* d = Data;
dVector3 c;
const dMatrix3& R = final_posr->R;
const dVector3& pos = final_posr->pos;
dMultiply0_331( c, R, d->AABBCenter );
dReal xrange = dFabs(R[0] * Data->AABBExtents[0]) +
dFabs(R[1] * Data->AABBExtents[1]) +
dFabs(R[2] * Data->AABBExtents[2]);
dReal yrange = dFabs(R[4] * Data->AABBExtents[0]) +
dFabs(R[5] * Data->AABBExtents[1]) +
dFabs(R[6] * Data->AABBExtents[2]);
dReal zrange = dFabs(R[8] * Data->AABBExtents[0]) +
dFabs(R[9] * Data->AABBExtents[1]) +
dFabs(R[10] * Data->AABBExtents[2]);
aabb[0] = c[0] + pos[0] - xrange;
aabb[1] = c[0] + pos[0] + xrange;
aabb[2] = c[1] + pos[1] - yrange;
aabb[3] = c[1] + pos[1] + yrange;
aabb[4] = c[2] + pos[2] - zrange;
aabb[5] = c[2] + pos[2] + zrange;
}
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->offset_posr) {
// move body such that body+offset = position
dVector3 world_offset;
dMultiply0_331(world_offset, g->body->posr.R, g->offset_posr->pos);
dBodySetPosition(g->body,
x - world_offset[0],
y - world_offset[1],
z - world_offset[2]);
}
else if (g->body) {
// this will call dGeomMoved (g), so we don't have to
dBodySetPosition (g->body,x,y,z);
}
else {
g->final_posr->pos[0] = x;
g->final_posr->pos[1] = y;
g->final_posr->pos[2] = z;
dGeomMoved (g);
}
}
void
dxJointUniversal::getAxes( dVector3 ax1, dVector3 ax2 )
{
// This says "ax1 = joint->node[0].body->posr.R * joint->axis1"
dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
if ( node[1].body )
{
dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
}
else
{
ax2[0] = axis2[0];
ax2[1] = axis2[1];
ax2[2] = axis2[2];
}
}
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] );
}
}
dReal
dxJointHinge2::measureAngle() const
{
dVector3 a1, a2;
dMultiply0_331( a1, node[1].body->posr.R, axis2 );
dMultiply1_331( a2, node[0].body->posr.R, a1 );
dReal x = dCalcVectorDot3( v1, a2 );
dReal y = dCalcVectorDot3( v2, a2 );
return -dAtan2( y, x );
}
void getAnchor( dxJoint *j, dVector3 result, dVector3 anchor1 )
{
if ( j->node[0].body )
{
dMultiply0_331( result, j->node[0].body->posr.R, anchor1 );
result[0] += j->node[0].body->posr.pos[0];
result[1] += j->node[0].body->posr.pos[1];
result[2] += j->node[0].body->posr.pos[2];
}
}
void setBall( dxJoint *joint, dxJoint::Info2 *info,
dVector3 anchor1, dVector3 anchor2 )
{
// anchor points in global coordinates with respect to body PORs.
dVector3 a1, a2;
int s = info->rowskip;
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
dMultiply0_331( a1, joint->node[0].body->posr.R, anchor1 );
dSetCrossMatrixMinus( info->J1a, a1, s );
if ( joint->node[1].body )
{
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 );
dSetCrossMatrixPlus( info->J2a, a2, s );
}
// set right hand side
dReal k = info->fps * info->erp;
if ( joint->node[1].body )
{
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( a2[j] + joint->node[1].body->posr.pos[j] -
a1[j] - joint->node[0].body->posr.pos[j] );
}
}
else
{
for ( int j = 0; j < 3; j++ )
{
info->c[j] = k * ( anchor2[j] - a1[j] -
joint->node[0].body->posr.pos[j] );
}
}
}
void setFixedOrientation( dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row )
{
int s = info->rowskip;
int start_index = start_row * s;
// 3 rows to make body rotations equal
info->J1a[start_index] = 1;
info->J1a[start_index + s + 1] = 1;
info->J1a[start_index + s*2+2] = 1;
if ( joint->node[1].body )
{
info->J2a[start_index] = -1;
info->J2a[start_index + s+1] = -1;
info->J2a[start_index + s*2+2] = -1;
}
// compute the right hand side. the first three elements will result in
// relative angular velocity of the two bodies - this is set to bring them
// back into alignment. the correcting angular velocity is
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * u
// = (erp*fps) * theta * u
// where rotation along unit length axis u by theta brings body 2's frame
// to qrel with respect to body 1's frame. using a small angle approximation
// for sin(), this gives
// angular_velocity = (erp*fps) * 2 * v
// where the quaternion of the relative rotation between the two bodies is
// q = [cos(theta/2) sin(theta/2)*u] = [s v]
// get qerr = relative rotation (rotation error) between two bodies
dQuaternion qerr, e;
if ( joint->node[1].body )
{
dQuaternion qq;
dQMultiply1( qq, joint->node[0].body->q, joint->node[1].body->q );
dQMultiply2( qerr, qq, qrel );
}
else
{
dQMultiply3( qerr, joint->node[0].body->q, qrel );
}
if ( qerr[0] < 0 )
{
qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small
qerr[2] = -qerr[2];
qerr[3] = -qerr[3];
}
dMultiply0_331( e, joint->node[0].body->posr.R, qerr + 1 ); // @@@ bad SIMD padding!
dReal k = info->fps * info->erp;
info->c[start_row] = 2 * k * e[0];
info->c[start_row+1] = 2 * k * e[1];
info->c[start_row+2] = 2 * k * e[2];
}
void drawGeom (dGeomID g, const dReal *pos, const dReal *R, int show_aabb)
{
if (!g) return;
if (!pos) pos = dGeomGetPosition (g);
if (!R) R = dGeomGetRotation (g);
int type = dGeomGetClass (g);
if (type == dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type == dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type == dCapsuleClass) {
dReal radius,length;
dGeomCapsuleGetParams (g,&radius,&length);
dsDrawCapsule (pos,R,length,radius);
}
else if (type == dCylinderClass) {
dReal radius,length;
dGeomCylinderGetParams (g,&radius,&length);
dsDrawCylinder (pos,R,length,radius);
}
else if (type == dGeomTransformClass) {
dGeomID g2 = dGeomTransformGetGeom (g);
const dReal *pos2 = dGeomGetPosition (g2);
const dReal *R2 = dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMultiply0_331 (actual_pos,R,pos2);
actual_pos[0] += pos[0];
actual_pos[1] += pos[1];
actual_pos[2] += pos[2];
dMultiply0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R,0);
}
if (show_aabb) {
// draw the bounding box for this geom
dReal aabb[6];
dGeomGetAABB (g,aabb);
dVector3 bbpos;
for (int i=0; i<3; i++) bbpos[i] = 0.5*(aabb[i*2] + aabb[i*2+1]);
dVector3 bbsides;
for (int j=0; j<3; j++) bbsides[j] = aabb[j*2+1] - aabb[j*2];
dMatrix3 RI;
dRSetIdentity (RI);
dsSetColorAlpha (1,0,0,0.5);
dsDrawBox (bbpos,RI,bbsides);
}
}
/*******************************************************************************
Function to draw a geometry object.
*******************************************************************************/
void GOdeObject::drawGeom( dGeomID g, const dReal *position, const dReal *orientation ) const
{
if( !g ) //If the geometry object is missing, end the function.
return;
if( !position ) //Position was not passed?
position = dGeomGetPosition( g ); //Then, get the geometry position.
if( !orientation ) //Orientation was not given?
orientation = dGeomGetRotation( g ); //And get existing geometry orientation.
int type = dGeomGetClass( g ); //Get the type of geometry.
if( type == dBoxClass ) //Is it a box?
{
dReal sides[3];
dGeomBoxGetLengths( g, sides ); //Get length of sides.
renderBox( sides, position, orientation ); //Render the actual box in environment.
}
if( type == dSphereClass ) //Is it a sphere?
{
dReal radius;
radius = dGeomSphereGetRadius( g ); //Get the radius.
renderSphere( radius, position, orientation ); //Render sphere in environment.
}
if( type == dCapsuleClass )
{
dReal radius;
dReal length;
dGeomCapsuleGetParams( g, &radius, &length ); //Get both radius and length.
renderCapsule( radius, length, position, orientation ); //Render capsule in environment.
}
if( type == dGeomTransformClass ) //Is it an embeded geom in a composite body.
{
dGeomID g2 = dGeomTransformGetGeom( g ); //Get the actual geometry inside the wrapper.
const dReal *position2 = dGeomGetPosition( g2 ); //Get position and orientation of wrapped geometry.
const dReal *orientation2 = dGeomGetRotation( g2 );
dVector3 actualPosition; //Real world coordinated position and orientation
dMatrix3 actualOrientation; //of the wrapped geometry.
dMultiply0_331( actualPosition, orientation, position2 ); //Get world coordinates of geometry position.
actualPosition[0] += position[0];
actualPosition[1] += position[1];
actualPosition[2] += position[2];
dMultiply0_333( actualOrientation, orientation, orientation2 ); //Get world coordinates of geom orientation.
drawGeom( g2, actualPosition, actualOrientation ); //Draw embeded geometry.
}
}
void dJointGetHinge2Axis2( dJointID j, dVector3 result )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, Hinge2 );
if ( joint->node[1].body )
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
}
}
dReal dJointGetScrewPosition ( dJointID j )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
// get axis1 in global coordinates
dVector3 ax1, q;
if (!joint->node[0].body)
return 0;
dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );
if (joint->node[1].body)
{
// get body2 + offset point in global coordinates
dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset );
for (int i = 0; i < 3; ++i )
q[i] = joint->node[0].body->posr.pos[i]
- q[i]
- joint->node[1].body->posr.pos[i];
}
else
{
q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0];
q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1];
q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2];
if ( joint->flags & dJOINT_REVERSE )
{
// N.B. it could have been simplier to only inverse the sign of
// the dCalcVectorDot3 result but this case is exceptional and
// doing the check for all case can decrease the performance.
ax1[0] = -ax1[0];
ax1[1] = -ax1[1];
ax1[2] = -ax1[2];
}
}
return dCalcVectorDot3 ( ax1, q );
}
void
dxJointFixed::getInfo2 ( dxJoint::Info2 *info )
{
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
info->cfm[5] = cfm;
}
int s = info->rowskip;
// Three rows for orientation
setFixedOrientation ( this, info, qrel, 3 );
// Three rows for position.
// set jacobian
info->J1l[0] = 1;
info->J1l[s+1] = 1;
info->J1l[2*s+2] = 1;
dVector3 ofs;
dMultiply0_331 ( ofs, node[0].body->posr.R, offset );
if ( node[1].body )
{
dSetCrossMatrixPlus( info->J1a, ofs, s );
info->J2l[0] = -1;
info->J2l[s+1] = -1;
info->J2l[2*s+2] = -1;
}
// set right hand side for the first three rows (linear)
dReal k = info->fps * info->erp;
if ( node[1].body )
{
for ( int j = 0; j < 3; j++ )
info->c[j] = k * ( node[1].body->posr.pos[j]
- node[0].body->posr.pos[j]
+ ofs[j] );
}
else
{
for ( int j = 0; j < 3; j++ )
info->c[j] = k * ( offset[j] - node[0].body->posr.pos[j] );
}
}
void dxGeom::computePosr()
{
// should only be recalced if we need to - ie offset from a body
dIASSERT(offset_posr);
dIASSERT(body);
dMultiply0_331 (final_posr->pos,body->posr.R,offset_posr->pos);
final_posr->pos[0] += body->posr.pos[0];
final_posr->pos[1] += body->posr.pos[1];
final_posr->pos[2] += body->posr.pos[2];
dMultiply0_333 (final_posr->R,body->posr.R,offset_posr->R);
}
void getWorldOffsetPosr(const dxPosR& body_posr, const dxPosR& world_posr, dxPosR& offset_posr)
{
dMatrix3 inv_body;
matrixInvert(body_posr.R, inv_body);
dMultiply0_333(offset_posr.R, inv_body, world_posr.R);
dVector3 world_offset;
world_offset[0] = world_posr.pos[0] - body_posr.pos[0];
world_offset[1] = world_posr.pos[1] - body_posr.pos[1];
world_offset[2] = world_posr.pos[2] - body_posr.pos[2];
dMultiply0_331(offset_posr.pos, inv_body, world_offset);
}
void getBodyPosr(const dxPosR& offset_posr, const dxPosR& final_posr, dxPosR& body_posr)
{
dMatrix3 inv_offset;
matrixInvert(offset_posr.R, inv_offset);
dMultiply0_333(body_posr.R, final_posr.R, inv_offset);
dVector3 world_offset;
dMultiply0_331(world_offset, body_posr.R, offset_posr.pos);
body_posr.pos[0] = final_posr.pos[0] - world_offset[0];
body_posr.pos[1] = final_posr.pos[1] - world_offset[1];
body_posr.pos[2] = final_posr.pos[2] - world_offset[2];
}
void
dxJointAMotor::computeEulerAngles( dVector3 ax[3] )
{
// assumptions:
// global axes already calculated --> ax
// axis[0] is relative to body 1 --> global ax[0]
// axis[2] is relative to body 2 --> global ax[2]
// ax[1] = ax[2] x ax[0]
// original ax[0] and ax[2] are perpendicular
// reference1 is perpendicular to ax[0] (in body 1 frame)
// reference2 is perpendicular to ax[2] (in body 2 frame)
// all ax[] and reference vectors are unit length
// calculate references in global frame
dVector3 ref1, ref2;
dMultiply0_331( ref1, node[0].body->posr.R, reference1 );
if ( node[1].body )
{
dMultiply0_331( ref2, node[1].body->posr.R, reference2 );
}
else
{
ref2[0] = reference2[0];
ref2[1] = reference2[1];
ref2[2] = reference2[2];
}
// get q perpendicular to both ax[0] and ref1, get first euler angle
dVector3 q;
dCalcVectorCross3( q, ax[0], ref1 );
angle[0] = -dAtan2( dCalcVectorDot3( ax[2], q ), dCalcVectorDot3( ax[2], ref1 ) );
// get q perpendicular to both ax[0] and ax[1], get second euler angle
dCalcVectorCross3( q, ax[0], ax[1] );
angle[1] = -dAtan2( dCalcVectorDot3( ax[2], ax[0] ), dCalcVectorDot3( ax[2], q ) );
// get q perpendicular to both ax[1] and ax[2], get third euler angle
dCalcVectorCross3( q, ax[1], ax[2] );
angle[2] = -dAtan2( dCalcVectorDot3( ref2, ax[1] ), dCalcVectorDot3( ref2, q ) );
}
void
dxJointAMotor::setEulerReferenceVectors()
{
if ( node[0].body && node[1].body )
{
dVector3 r; // axis[2] and axis[0] in global coordinates
dMultiply0_331( r, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference1, node[0].body->posr.R, r );
dMultiply0_331( r, node[0].body->posr.R, axis[0] );
dMultiply1_331( reference2, node[1].body->posr.R, r );
} else {
// We want to handle angular motors attached to passive geoms
// Replace missing node.R with identity
if (node[0].body) {
dMultiply1_331( reference1, node[0].body->posr.R, axis[2] );
dMultiply0_331( reference2, node[0].body->posr.R, axis[0] );
} else if (node[1].body) {
dMultiply0_331( reference1, node[1].body->posr.R, axis[2] );
dMultiply1_331( reference2, node[1].body->posr.R, axis[0] );
}
}
}
void getAxis2( dxJoint *j, dVector3 result, dVector3 axis2 )
{
if ( j->node[1].body )
{
dMultiply0_331( result, j->node[1].body->posr.R, axis2 );
}
else
{
result[0] = axis2[0];
result[1] = axis2[1];
result[2] = axis2[2];
}
}
void
dxJointLMotor::computeGlobalAxes( dVector3 ax[3] )
{
for ( int i = 0; i < num; i++ )
{
if ( rel[i] == 1 )
{
dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
}
else if ( rel[i] == 2 )
{
if ( node[1].body ) // jds: don't assert, just ignore
{
dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
}
}
else
{
ax[i][0] = axis[i][0];
ax[i][1] = axis[i][1];
ax[i][2] = axis[i][2];
}
}
}
