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